payloads/libpayload/drivers/storage/ata.c

   1 /*
   2  *
   3  * Copyright (C) 2012 secunet Security Networks AG
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   8  * 1. Redistributions of source code must retain the above copyright
   9  *    notice, this list of conditions and the following disclaimer.
  10  * 2. Redistributions in binary form must reproduce the above copyright
  11  *    notice, this list of conditions and the following disclaimer in the
  12  *    documentation and/or other materials provided with the distribution.
  13  * 3. The name of the author may not be used to endorse or promote products
  14  *    derived from this software without specific prior written permission.
  15  *
  16  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  17  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  18  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  19  * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  20  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  21  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  22  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  23  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  24  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  25  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  26  * SUCH DAMAGE.
  27  */
  28
  29 #include <libpayload.h>
  30 #include <stdint.h>
  31 #include <string.h>
  32 #include <stdio.h>
  33
  34 #include <storage/ata.h>
  35
  36 /** Reads non-sector-aligned blocks of 512 bytes. */
  37 static ssize_t ata_read_unaligned(ata_dev_t *const dev,
  38                                   const lba_t blk_start, size_t blk_count,
  39                                   u8 *blk_buf)
  40 {
  41         ssize_t ret = 0;
  42         lba_t sec_start;
  43         size_t blk_residue;
  44
  45         u8 *const sec_buf = malloc(dev->sector_size);
  46         if (sec_buf == NULL)
  47                 return -1;
  48
  49         const size_t shift = dev->sector_size_shift - 9;
  50         const size_t mask = (dev->sector_size >> 9) - 1;
  51
  52         /* Sector aligned start block. */
  53         const lba_t blk_aligned = blk_start & ~mask;
  54
  55         /* First sector to read from. */
  56         sec_start = blk_aligned >> shift;
  57
  58         /* Calculate and read residue before sector aligned blocks. */
  59         blk_residue = MIN(blk_start - blk_aligned, blk_count);
  60         if (blk_residue) {
  61                 if (dev->read_sectors(dev, sec_start, 1, sec_buf) != 1)
  62                         goto _free_ret;
  63
  64                 const size_t bytes = blk_residue << 9;
  65                 memcpy(blk_buf, sec_buf + (dev->sector_size - bytes), bytes);
  66                 blk_count -= blk_residue;
  67                 blk_buf += bytes;
  68                 ++sec_start;
  69
  70                 ret = blk_residue;
  71                 if (blk_count == 0)
  72                         goto _free_ret;
  73         }
  74
  75         /* Read all sector aligned blocks. */
  76         const size_t sec_count = (blk_count & ~mask) >> shift;
  77         const int sec_read = dev->read_sectors(
  78                                 dev, sec_start, sec_count, blk_buf);
  79         if (sec_read < 0)
  80                 goto _free_ret;
  81         ret += sec_read << shift;
  82         if (sec_read != sec_count)
  83                 goto _free_ret;
  84
  85         /* Calculate and read residue. */
  86         blk_residue = blk_count & mask;
  87         if (blk_residue) {
  88                 sec_start += sec_read;
  89                 blk_buf += sec_read << dev->sector_size_shift;
  90
  91                 if (dev->read_sectors(dev, sec_start, 1, sec_buf) != 1)
  92                         goto _free_ret;
  93
  94                 const size_t bytes = blk_residue << 9;
  95                 memcpy(blk_buf, sec_buf, bytes);
  96                 ret += blk_residue;
  97         }
  98
  99 _free_ret:
 100         free(sec_buf);
 101         return ret;
 102 }
 103
 104 static ssize_t ata_read512(storage_dev_t *_dev,
 105                            const lba_t start, const size_t count,
 106                            unsigned char *const buf)
 107 {
 108         ata_dev_t *const dev = (ata_dev_t *)_dev;
 109
 110         if (dev->read_sectors == NULL) {
 111                 printf("ata: No read support implemented.\n");
 112                 return -1;
 113         }
 114
 115         if (dev->sector_size == 512) {
 116                 return dev->read_sectors(dev, start, count, buf);
 117         } else if (dev->sector_size > 512) {
 118                 /* Sector size has to be a power of two. */
 119                 const size_t mask = (dev->sector_size >> 9) - 1;
 120                 if (!(start & mask) && !(count & mask)) {
 121                         const size_t shift = dev->sector_size_shift - 9;
 122                         const ssize_t ret = dev->read_sectors(dev,
 123                                         start >> shift, count >> shift, buf);
 124                         if (ret < 0)
 125                                 return ret;
 126                         else
 127                                 return ret << shift;
 128                 } else {
 129                         return ata_read_unaligned(dev, start, count, buf);
 130                 }
 131         } else {
 132                 printf("ata: No support for sectors smaller than 512 bytes.\n");
 133                 return -1;
 134         }
 135 }
 136
 137 static ssize_t ata_write512(storage_dev_t *const dev,
 138                             const lba_t start, const size_t count,
 139                             const unsigned char *const buf)
 140 {
 141         printf("ata: No write support implemented.\n");
 142         return -1;
 143 }
 144
 145 void ata_initialize_storage_ops(ata_dev_t *const dev)
 146 {
 147         dev->storage_dev.read_blocks512 = ata_read512;
 148         dev->storage_dev.write_blocks512 = ata_write512;
 149 }
 150
 151 int ata_set_sector_size(ata_dev_t *const dev, u32 sector_size)
 152 {
 153         if (!sector_size || (sector_size & (sector_size - 1))) {
 154                 printf("ata: Sector size is not a power of two (%u).\n",
 155                                                                 sector_size);
 156                 return -1;
 157         }
 158         dev->sector_size = sector_size;
 159         dev->sector_size_shift = 0;
 160         while (sector_size >>= 1)
 161                 ++dev->sector_size_shift;
 162
 163         return 0;
 164 }
 165
 166 static int ata_decode_sector_size(ata_dev_t *const dev, const u16 *const id)
 167 {
 168         u32 size;
 169         if ((id[ATA_ID_SECTOR_SIZE] & ((3 << 14) | (1 << 12)))
 170                         != ((1 << 14) | (1 << 12)))
 171                 size = DEFAULT_ATA_SECTOR_SIZE;
 172         else
 173                 size = (id[ATA_ID_LOGICAL_SECTOR_SIZE] |
 174                         (id[ATA_ID_LOGICAL_SECTOR_SIZE + 1] << 16)) << 1;
 175
 176         return ata_set_sector_size(dev, size);
 177 }
 178
 179 /**
 180  * Copies n-1 bytes from src to dest swapping each two bytes, removes
 181  * trailing spaces and terminates dest with '\0'.
 182  */
 183 char *ata_strncpy(char *const dest, const u16 *const src, const size_t n)
 184 {
 185         int i;
 186
 187         for (i = 0; i < (n - 1); i += 2) {
 188                 dest[i] = ((const char *)src)[i + 1];
 189                 dest[i + 1] = ((const char *)src)[i];
 190         }
 191
 192         for (i = n - 2; i >= 0; --i)
 193                 if (dest[i] != ' ')
 194                         break;
 195         dest[i + 1] = '\0';
 196
 197         return dest;
 198 }
 199
 200 int ata_attach_device(ata_dev_t *const dev, const storage_port_t port_type)
 201 {
 202         u16 id[256];
 203
 204         dev->identify_cmd = ATA_IDENTIFY_DEVICE;
 205         if (dev->identify(dev, (u8 *)id))
 206                 return -1;
 207
 208         char fw[9], model[41];
 209         ata_strncpy(fw, id + 23, sizeof(fw));
 210         ata_strncpy(model, id + 27, sizeof(model));
 211         printf("ata: Identified %s [%s]\n", model, fw);
 212
 213 #if CONFIG(LP_STORAGE_64BIT_LBA)
 214         if (id[ATA_CMDS_AND_FEATURE_SETS + 1] & (1 << 10)) {
 215                 printf("ata: Support for LBA-48 enabled.\n");
 216                 dev->read_cmd = ATA_READ_DMA_EXT;
 217         } else {
 218                 dev->read_cmd = ATA_READ_DMA;
 219         }
 220 #else
 221         dev->read_cmd = ATA_READ_DMA;
 222 #endif
 223
 224         if (ata_decode_sector_size(dev, id))
 225                 return -1;
 226
 227         dev->storage_dev.port_type = port_type;
 228         ata_initialize_storage_ops(dev);
 229
 230         return storage_attach_device(&dev->storage_dev);
 231 }