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