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. */
ata_read_unaligned(ata_dev_t * const dev,const lba_t blk_start,size_t blk_count,u8 * blk_buf)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
ata_read512(storage_dev_t * _dev,const lba_t start,const size_t count,unsigned char * const buf)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
ata_write512(storage_dev_t * const dev,const lba_t start,const size_t count,const unsigned char * const buf)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
ata_initialize_storage_ops(ata_dev_t * const dev)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
ata_set_sector_size(ata_dev_t * const dev,u32 sector_size)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
ata_decode_sector_size(ata_dev_t * const dev,const u16 * const id)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 */
ata_strncpy(char * const dest,const u16 * const src,const size_t n)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
ata_attach_device(ata_dev_t * const dev,const storage_port_t port_type)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 }
232