/*
 * This file is part of the libpayload project.
 *
 * Copyright (C) 2012 secunet Security Networks AG
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#include <libpayload.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>

#include <storage/ata.h>


/** Reads non-sector-aligned blocks of 512 bytes. */
static ssize_t ata_read_unaligned(ata_dev_t *const dev,
				  const lba_t blk_start, size_t blk_count,
				  u8 *blk_buf)
{
	ssize_t ret = 0;
	lba_t sec_start;
	size_t blk_residue;

	u8 *const sec_buf = malloc(dev->sector_size);
	if (sec_buf == NULL)
		return -1;

	const size_t shift = dev->sector_size_shift - 9;
	const size_t mask = (dev->sector_size >> 9) - 1;

	/* Sector aligned start block. */
	const lba_t blk_aligned = blk_start & ~mask;

	/* First sector to read from. */
	sec_start = blk_aligned >> shift;

	/* Calculate and read residue before sector aligned blocks. */
	blk_residue = MIN(blk_start - blk_aligned, blk_count);
	if (blk_residue) {
		if (dev->read_sectors(dev, sec_start, 1, sec_buf) != 1)
			goto _free_ret;

		const size_t bytes = blk_residue << 9;
		memcpy(blk_buf, sec_buf + (dev->sector_size - bytes), bytes);
		blk_count -= blk_residue;
		blk_buf += bytes;
		++sec_start;

		ret = blk_residue;
		if (blk_count == 0)
			goto _free_ret;
	}

	/* Read all sector aligned blocks. */
	const size_t sec_count = (blk_count & ~mask) >> shift;
	const int sec_read = dev->read_sectors(
				dev, sec_start, sec_count, blk_buf);
	if (sec_read < 0)
		goto _free_ret;
	ret += sec_read << shift;
	if (sec_read != sec_count)
		goto _free_ret;

	/* Calculate and read residue. */
	blk_residue = blk_count & mask;
	if (blk_residue) {
		sec_start += sec_read;
		blk_buf += sec_read << dev->sector_size_shift;

		if (dev->read_sectors(dev, sec_start, 1, sec_buf) != 1)
			goto _free_ret;

		const size_t bytes = blk_residue << 9;
		memcpy(blk_buf, sec_buf, bytes);
		ret += blk_residue;
	}

_free_ret:
	free(sec_buf);
	return ret;
}

static ssize_t ata_read512(storage_dev_t *_dev,
			   const lba_t start, const size_t count,
			   unsigned char *const buf)
{
	ata_dev_t *const dev = (ata_dev_t *)_dev;

	if (dev->read_sectors == NULL) {
		printf("ata: No read support implemented.\n");
		return -1;
	}

	if (dev->sector_size == 512) {
		return dev->read_sectors(dev, start, count, buf);
	} else if (dev->sector_size > 512) {
		/* Sector size has to be a power of two. */
		const size_t mask = (dev->sector_size >> 9) - 1;
		if (!(start & mask) && !(count & mask)) {
			const size_t shift = dev->sector_size_shift - 9;
			const ssize_t ret = dev->read_sectors(dev,
					start >> shift, count >> shift, buf);
			if (ret < 0)
				return ret;
			else
				return ret << shift;
		} else {
			return ata_read_unaligned(dev, start, count, buf);
		}
	} else {
		printf("ata: No support for sectors smaller than 512 bytes.\n");
		return -1;
	}
}

static ssize_t ata_write512(storage_dev_t *const dev,
			    const lba_t start, const size_t count,
			    const unsigned char *const buf)
{
	printf("ata: No write support implemented.\n");
	return -1;
}

void ata_initialize_storage_ops(ata_dev_t *const dev)
{
	dev->storage_dev.read_blocks512 = ata_read512;
	dev->storage_dev.write_blocks512 = ata_write512;
}

int ata_set_sector_size(ata_dev_t *const dev, u32 sector_size)
{
	if (!sector_size || (sector_size & (sector_size - 1))) {
		printf("ata: Sector size is not a power of two (%u).\n",
								sector_size);
		return -1;
	}
	dev->sector_size = sector_size;
	dev->sector_size_shift = 0;
	while (sector_size >>= 1)
		++dev->sector_size_shift;

	return 0;
}

static int ata_decode_sector_size(ata_dev_t *const dev, const u16 *const id)
{
	u32 size;
	if ((id[ATA_ID_SECTOR_SIZE] & ((3 << 14) | (1 << 12)))
			!= ((1 << 14) | (1 << 12)))
		size = DEFAULT_ATA_SECTOR_SIZE;
	else
		size = (id[ATA_ID_LOGICAL_SECTOR_SIZE] |
			(id[ATA_ID_LOGICAL_SECTOR_SIZE + 1] << 16)) << 1;

	return ata_set_sector_size(dev, size);
}

/**
 * Copies n-1 bytes from src to dest swapping each two bytes, removes
 * trailing spaces and terminates dest with '\0'.
 */
char *ata_strncpy(char *const dest, const u16 *const src, const size_t n)
{
	int i;

	for (i = 0; i < (n - 1); i += 2) {
		dest[i] = ((const char *)src)[i + 1];
		dest[i + 1] = ((const char *)src)[i];
	}

	for (i = n - 2; i >= 0; --i)
		if (dest[i] != ' ')
			break;
	dest[i + 1] = '\0';

	return dest;
}

int ata_attach_device(ata_dev_t *const dev, const storage_port_t port_type)
{
	u16 id[256];

	dev->identify_cmd = ATA_IDENTIFY_DEVICE;
	if (dev->identify(dev, (u8 *)id))
		return -1;

	char fw[9], model[41];
	ata_strncpy(fw, id + 23, sizeof(fw));
	ata_strncpy(model, id + 27, sizeof(model));
	printf("ata: Identified %s [%s]\n", model, fw);

#if CONFIG(LP_STORAGE_64BIT_LBA)
	if (id[ATA_CMDS_AND_FEATURE_SETS + 1] & (1 << 10)) {
		printf("ata: Support for LBA-48 enabled.\n");
		dev->read_cmd = ATA_READ_DMA_EXT;
	} else {
		dev->read_cmd = ATA_READ_DMA;
	}
#else
	dev->read_cmd = ATA_READ_DMA;
#endif

	if (ata_decode_sector_size(dev, id))
		return -1;

	dev->storage_dev.port_type = port_type;
	ata_initialize_storage_ops(dev);

	return storage_attach_device(&dev->storage_dev);
}