coreboot-kgpe-d16/payloads/libpayload/drivers/storage/ata.c

234 lines
6.4 KiB
C

/*
* 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);
}