| 1 | #include <etherboot.h>
|
|---|
| 2 |
|
|---|
| 3 | #include <lib.h>
|
|---|
| 4 | #include <fs.h>
|
|---|
| 5 |
|
|---|
| 6 | #define DEBUG_THIS DEBUG_BLOCKDEV
|
|---|
| 7 | #include <debug.h>
|
|---|
| 8 |
|
|---|
| 9 | #define NUM_CACHE 64
|
|---|
| 10 | static unsigned char buf_cache[NUM_CACHE][512];
|
|---|
| 11 | static unsigned long cache_sect[NUM_CACHE];
|
|---|
| 12 |
|
|---|
| 13 | static char dev_name[256];
|
|---|
| 14 |
|
|---|
| 15 | int dev_type = -1;
|
|---|
| 16 | int dev_drive = -1;
|
|---|
| 17 | unsigned long part_start;
|
|---|
| 18 | unsigned long part_length;
|
|---|
| 19 | int using_devsize;
|
|---|
| 20 |
|
|---|
| 21 | static inline int has_pc_part_magic(unsigned char *sect)
|
|---|
| 22 | {
|
|---|
| 23 | return sect[510]==0x55 && sect[511]==0xAA;
|
|---|
| 24 | }
|
|---|
| 25 |
|
|---|
| 26 | static inline int is_pc_extended_part(unsigned char type)
|
|---|
| 27 | {
|
|---|
| 28 | return type==5 || type==0xf || type==0x85;
|
|---|
| 29 | }
|
|---|
| 30 |
|
|---|
| 31 | /* IBM-PC/MS-DOS style partitioning scheme */
|
|---|
| 32 | static int open_pc_partition(int part, unsigned long *start_p,
|
|---|
| 33 | unsigned long *length_p)
|
|---|
| 34 | {
|
|---|
| 35 | /* Layout of PC partition table */
|
|---|
| 36 | struct pc_partition {
|
|---|
| 37 | unsigned char boot;
|
|---|
| 38 | unsigned char head;
|
|---|
| 39 | unsigned char sector;
|
|---|
| 40 | unsigned char cyl;
|
|---|
| 41 | unsigned char type;
|
|---|
| 42 | unsigned char e_head;
|
|---|
| 43 | unsigned char e_sector;
|
|---|
| 44 | unsigned char e_cyl;
|
|---|
| 45 | unsigned char start_sect[4]; /* unaligned little endian */
|
|---|
| 46 | unsigned char nr_sects[4]; /* ditto */
|
|---|
| 47 | } *p;
|
|---|
| 48 | unsigned char buf[512];
|
|---|
| 49 |
|
|---|
| 50 | /* PC partition probe */
|
|---|
| 51 | if (!devread(0, 0, sizeof(buf), buf)) {
|
|---|
| 52 | debug("device read failed\n");
|
|---|
| 53 | return 0;
|
|---|
| 54 | }
|
|---|
| 55 | if (!has_pc_part_magic(buf)) {
|
|---|
| 56 | debug("pc partition magic number not found\n");
|
|---|
| 57 | //debug_hexdump(buf, 512);
|
|---|
| 58 | return PARTITION_UNKNOWN;
|
|---|
| 59 | }
|
|---|
| 60 | p = (struct pc_partition *) (buf + 0x1be);
|
|---|
| 61 | if (part < 4) {
|
|---|
| 62 | /* Primary partition */
|
|---|
| 63 | p += part;
|
|---|
| 64 | if (p->type==0 || is_pc_extended_part(p->type)) {
|
|---|
| 65 | printf("Partition %d does not exist\n", part+1);
|
|---|
| 66 | return 0;
|
|---|
| 67 | }
|
|---|
| 68 | *start_p = get_le32(p->start_sect);
|
|---|
| 69 | *length_p = get_le32(p->nr_sects);
|
|---|
| 70 | return 1;
|
|---|
| 71 | } else {
|
|---|
| 72 | /* Extended partition */
|
|---|
| 73 | int i;
|
|---|
| 74 | int cur_part;
|
|---|
| 75 | unsigned long ext_start, cur_table;
|
|---|
| 76 | /* Search for the extended partition
|
|---|
| 77 | * which contains logical partitions */
|
|---|
| 78 | for (i = 0; i < 4; i++) {
|
|---|
| 79 | if (is_pc_extended_part(p[i].type))
|
|---|
| 80 | break;
|
|---|
| 81 | }
|
|---|
| 82 | if (i >= 4) {
|
|---|
| 83 | printf("Extended partition not found\n");
|
|---|
| 84 | return 0;
|
|---|
| 85 | }
|
|---|
| 86 | debug("Extended partition at %d\n", i+1);
|
|---|
| 87 | /* Visit each logical partition labels */
|
|---|
| 88 | ext_start = get_le32(p[i].start_sect);
|
|---|
| 89 | cur_table = ext_start;
|
|---|
| 90 | cur_part = 4;
|
|---|
| 91 | for (;;) {
|
|---|
| 92 | debug("cur_part=%d at %lu\n", cur_part, cur_table);
|
|---|
| 93 | if (!devread(cur_table, 0, sizeof(buf), buf))
|
|---|
| 94 | return 0;
|
|---|
| 95 | if (!has_pc_part_magic(buf)) {
|
|---|
| 96 | debug("no magic\n");
|
|---|
| 97 | break;
|
|---|
| 98 | }
|
|---|
| 99 |
|
|---|
| 100 | p = (struct pc_partition *) (buf + 0x1be);
|
|---|
| 101 | /* First entry is the logical partition */
|
|---|
| 102 | if (cur_part == part) {
|
|---|
| 103 | if (p->type==0) {
|
|---|
| 104 | printf("Partition %d is empty\n", part+1);
|
|---|
| 105 | return 0;
|
|---|
| 106 | }
|
|---|
| 107 | *start_p = cur_table + get_le32(p->start_sect);
|
|---|
| 108 | *length_p = get_le32(p->nr_sects);
|
|---|
| 109 | return 1;
|
|---|
| 110 | }
|
|---|
| 111 | /* Second entry is link to next partition */
|
|---|
| 112 | if (!is_pc_extended_part(p[1].type)) {
|
|---|
| 113 | debug("no link\n");
|
|---|
| 114 | break;
|
|---|
| 115 | }
|
|---|
| 116 | cur_table = ext_start + get_le32(p[1].start_sect);
|
|---|
| 117 |
|
|---|
| 118 | cur_part++;
|
|---|
| 119 | }
|
|---|
| 120 | printf("Logical partition %d not exist\n", part+1);
|
|---|
| 121 | return 0;
|
|---|
| 122 | }
|
|---|
| 123 | }
|
|---|
| 124 |
|
|---|
| 125 | static void flush_cache(void)
|
|---|
| 126 | {
|
|---|
| 127 | int i;
|
|---|
| 128 | for (i = 0; i < NUM_CACHE; i++)
|
|---|
| 129 | cache_sect[i] = (unsigned long) -1;
|
|---|
| 130 | }
|
|---|
| 131 |
|
|---|
| 132 | static int parse_device_name(const char *name, int *type, int *drive,
|
|---|
| 133 | int *part, uint64_t *offset, uint64_t *length)
|
|---|
| 134 | {
|
|---|
| 135 | *offset = *length = 0;
|
|---|
| 136 |
|
|---|
| 137 | if (memcmp(name, "hd", 2) == 0) {
|
|---|
| 138 | *type = DISK_IDE;
|
|---|
| 139 | name += 2;
|
|---|
| 140 | if (*name < 'a' || *name > 'z') {
|
|---|
| 141 | printf("Invalid drive\n");
|
|---|
| 142 | return 0;
|
|---|
| 143 | }
|
|---|
| 144 | *drive = *name - 'a';
|
|---|
| 145 | name++;
|
|---|
| 146 | } else if (memcmp(name, "mem", 3) == 0) {
|
|---|
| 147 | *type = DISK_MEM;
|
|---|
| 148 | name += 3;
|
|---|
| 149 | *drive = 0;
|
|---|
| 150 | } else if (memcmp(name, "ud", 2) == 0) {
|
|---|
| 151 | *type = DISK_USB;
|
|---|
| 152 | name += 2;
|
|---|
| 153 | if (*name < 'a' || *name > 'z') {
|
|---|
| 154 | printf("Invalid drive\n");
|
|---|
| 155 | return 0;
|
|---|
| 156 | }
|
|---|
| 157 | *drive = *name - 'a';
|
|---|
| 158 | name++;
|
|---|
| 159 | } else {
|
|---|
| 160 | printf("Unknown device type\n");
|
|---|
| 161 | return 0;
|
|---|
| 162 | }
|
|---|
| 163 |
|
|---|
| 164 | *part = (int) simple_strtoull(name, (char **)&name, 0);
|
|---|
| 165 |
|
|---|
| 166 | if (*name == '@') {
|
|---|
| 167 | name++;
|
|---|
| 168 | *offset = strtoull_with_suffix(name, (char **)&name, 0);
|
|---|
| 169 | if (*name == ',')
|
|---|
| 170 | *length = strtoull_with_suffix(name+1, (char **)&name, 0);
|
|---|
| 171 | // debug("offset=%#Lx length=%#Lx\n", *offset, *length);
|
|---|
| 172 | }
|
|---|
| 173 |
|
|---|
| 174 | if (*name != '\0') {
|
|---|
| 175 | printf("Can't parse device name\n");
|
|---|
| 176 | return 0;
|
|---|
| 177 | }
|
|---|
| 178 |
|
|---|
| 179 | return 1;
|
|---|
| 180 | }
|
|---|
| 181 |
|
|---|
| 182 | int devopen(const char *name, int *reopen)
|
|---|
| 183 | {
|
|---|
| 184 | int type, drive, part;
|
|---|
| 185 | uint64_t offset, length;
|
|---|
| 186 | uint32_t disk_size = 0;
|
|---|
| 187 |
|
|---|
| 188 | /* Don't re-open the device that's already open */
|
|---|
| 189 | if (strcmp(name, dev_name) == 0) {
|
|---|
| 190 | debug("already open\n");
|
|---|
| 191 | *reopen = 1;
|
|---|
| 192 | return 1;
|
|---|
| 193 | }
|
|---|
| 194 | *reopen = 0;
|
|---|
| 195 |
|
|---|
| 196 | if (!parse_device_name(name, &type, &drive, &part, &offset, &length)) {
|
|---|
| 197 | debug("failed to parse device name: %s\n", name);
|
|---|
| 198 | return 0;
|
|---|
| 199 | }
|
|---|
| 200 |
|
|---|
| 201 | /* Do simple sanity check first */
|
|---|
| 202 | if (offset & 0x1ff) {
|
|---|
| 203 | printf("Device offset must be multiple of 512\n");
|
|---|
| 204 | return 0;
|
|---|
| 205 | }
|
|---|
| 206 | if (length & 0x1ff) {
|
|---|
| 207 | debugx("WARNING: length is rounded up to multiple of 512\n");
|
|---|
| 208 | length = (length + 0x1ff) & ~0x1ff;
|
|---|
| 209 | }
|
|---|
| 210 |
|
|---|
| 211 | switch (type) {
|
|---|
| 212 | #ifdef IDE_DISK
|
|---|
| 213 | case DISK_IDE:
|
|---|
| 214 | if (ide_probe(drive) != 0) {
|
|---|
| 215 | debug("failed to open ide\n");
|
|---|
| 216 | return 0;
|
|---|
| 217 | }
|
|---|
| 218 | disk_size = (uint32_t) -1; /* FIXME */
|
|---|
| 219 | break;
|
|---|
| 220 | #endif
|
|---|
| 221 | case DISK_MEM:
|
|---|
| 222 | disk_size = 1 << (32 - 9); /* 4GB/512-byte */
|
|---|
| 223 | break;
|
|---|
| 224 | #ifdef USB_DISK
|
|---|
| 225 | case DISK_USB:
|
|---|
| 226 | if (usb_probe(drive) != 0) {
|
|---|
| 227 | debug("failed to open usb\n");
|
|---|
| 228 | return 0;
|
|---|
| 229 | }
|
|---|
| 230 | disk_size = (uint32_t) -1; /* FIXME */
|
|---|
| 231 | break;
|
|---|
| 232 | #endif
|
|---|
| 233 | default:
|
|---|
| 234 | printf("Unknown device type %d\n", type);
|
|---|
| 235 | return 0;
|
|---|
| 236 | }
|
|---|
| 237 |
|
|---|
| 238 | if (dev_type != type || dev_drive != drive)
|
|---|
| 239 | flush_cache();
|
|---|
| 240 |
|
|---|
| 241 | /* start with whole disk */
|
|---|
| 242 | dev_type = type;
|
|---|
| 243 | dev_drive = drive;
|
|---|
| 244 | part_start = 0;
|
|---|
| 245 | part_length = disk_size;
|
|---|
| 246 | using_devsize = 1;
|
|---|
| 247 |
|
|---|
| 248 | if (part != 0) {
|
|---|
| 249 | /* partition is specified */
|
|---|
| 250 | int ret;
|
|---|
| 251 | ret = open_pc_partition(part - 1, &part_start, &part_length);
|
|---|
| 252 | if (ret == PARTITION_UNKNOWN) {
|
|---|
| 253 | ret = open_eltorito_image(part - 1, &part_start, &part_length);
|
|---|
| 254 | if (ret == PARTITION_UNKNOWN) {
|
|---|
| 255 | printf("Unrecognized partitioning scheme\n");
|
|---|
| 256 | return 0;
|
|---|
| 257 | }
|
|---|
| 258 | }
|
|---|
| 259 | if (ret == 0) {
|
|---|
| 260 | debug("can't open partition %d\n", part);
|
|---|
| 261 | return 0;
|
|---|
| 262 | }
|
|---|
| 263 |
|
|---|
| 264 | debug("Partition %d start %lu length %lu\n", part,
|
|---|
| 265 | part_start, part_length);
|
|---|
| 266 | }
|
|---|
| 267 |
|
|---|
| 268 | if (offset) {
|
|---|
| 269 | if (offset >= (uint64_t) part_length << 9) {
|
|---|
| 270 | printf("Device offset is too high\n");
|
|---|
| 271 | return 0;
|
|---|
| 272 | }
|
|---|
| 273 | part_start += offset >> 9;
|
|---|
| 274 | part_length -= offset >> 9;
|
|---|
| 275 | debug("after offset: start %lu, length %lu\n", part_start, part_length);
|
|---|
| 276 | }
|
|---|
| 277 |
|
|---|
| 278 | if (length) {
|
|---|
| 279 | if (length > (uint64_t) part_length << 9) {
|
|---|
| 280 | printf("Specified length exceeds the size of device\n");
|
|---|
| 281 | return 0;
|
|---|
| 282 | }
|
|---|
| 283 | part_length = length >> 9;
|
|---|
| 284 | debug("after length: length %lu\n", part_length);
|
|---|
| 285 | using_devsize = 0;
|
|---|
| 286 | }
|
|---|
| 287 |
|
|---|
| 288 | strncpy(dev_name, name, sizeof(dev_name)-1);
|
|---|
| 289 |
|
|---|
| 290 | return 1;
|
|---|
| 291 | }
|
|---|
| 292 |
|
|---|
| 293 | /* Read a sector from opened device with simple/stupid buffer cache */
|
|---|
| 294 | static void *read_sector(unsigned long sector)
|
|---|
| 295 | {
|
|---|
| 296 | unsigned int hash;
|
|---|
| 297 | void *buf;
|
|---|
| 298 | int i;
|
|---|
| 299 |
|
|---|
| 300 | /* If reading memory, just return the memory as the buffer */
|
|---|
| 301 | if (dev_type == DISK_MEM) {
|
|---|
| 302 | unsigned long phys = sector << 9;
|
|---|
| 303 | //debug("mem: %#lx\n", phys);
|
|---|
| 304 | return phys_to_virt(phys);
|
|---|
| 305 | }
|
|---|
| 306 |
|
|---|
| 307 | /* Search in the cache */
|
|---|
| 308 | hash = sector % NUM_CACHE;
|
|---|
| 309 | buf = buf_cache[hash];
|
|---|
| 310 | if (cache_sect[hash] != sector) {
|
|---|
| 311 | cache_sect[hash] = (unsigned long) -1;
|
|---|
| 312 | switch (dev_type) {
|
|---|
| 313 | #ifdef IDE_DISK
|
|---|
| 314 | case DISK_IDE:
|
|---|
| 315 | if (ide_read(dev_drive, sector, buf) != 0)
|
|---|
| 316 | goto readerr;
|
|---|
| 317 | break;
|
|---|
| 318 | #endif
|
|---|
| 319 | #ifdef USB_DISK
|
|---|
| 320 | case DISK_USB:
|
|---|
| 321 | if (usb_read(dev_drive, sector, buf) != 0)
|
|---|
| 322 | goto readerr;
|
|---|
| 323 | break;
|
|---|
| 324 | #endif
|
|---|
| 325 | default:
|
|---|
| 326 | printf("read_sector: device not open\n");
|
|---|
| 327 | return 0;
|
|---|
| 328 | }
|
|---|
| 329 | cache_sect[hash] = sector;
|
|---|
| 330 | }
|
|---|
| 331 | #if 0
|
|---|
| 332 | printf("in read_sector:\n");
|
|---|
| 333 | for(i=0;i<128;i++) {
|
|---|
| 334 | if((i%4)==0) printf("\n %08x:",i*4);
|
|---|
| 335 | printf(" %08x ",(uint32_t)*((uint32_t *)buf+i));
|
|---|
| 336 | }
|
|---|
| 337 | #endif
|
|---|
| 338 |
|
|---|
| 339 | return buf;
|
|---|
| 340 |
|
|---|
| 341 | readerr:
|
|---|
| 342 | printf("Disk read error dev_type=%d drive=%d sector=%x\n",
|
|---|
| 343 | dev_type, dev_drive, sector);
|
|---|
| 344 | dev_name[0] = '\0'; /* force re-open the device next time */
|
|---|
| 345 | return 0;
|
|---|
| 346 | }
|
|---|
| 347 |
|
|---|
| 348 | int devread(unsigned long sector, unsigned long byte_offset,
|
|---|
| 349 | unsigned long byte_len, void *buf)
|
|---|
| 350 | {
|
|---|
| 351 | char *sector_buffer;
|
|---|
| 352 | char *dest = buf;
|
|---|
| 353 | unsigned long len;
|
|---|
| 354 | int i;
|
|---|
| 355 |
|
|---|
| 356 | sector += byte_offset >> 9;
|
|---|
| 357 | byte_offset &= 0x1ff;
|
|---|
| 358 |
|
|---|
| 359 | if (sector + ((byte_len + 0x1ff) >> 9) > part_length) {
|
|---|
| 360 | printf("Attempt to read out of device/partition\n");
|
|---|
| 361 | debug("sector=%x part_length=%x byte_len=%x\n",
|
|---|
| 362 | sector, part_length, byte_len);
|
|---|
| 363 | return 0;
|
|---|
| 364 | }
|
|---|
| 365 |
|
|---|
| 366 | while (byte_len > 0) {
|
|---|
| 367 | sector_buffer = read_sector(part_start + sector);
|
|---|
| 368 | if (!sector_buffer) {
|
|---|
| 369 | debug("read sector failed\n");
|
|---|
| 370 | return 0;
|
|---|
| 371 | }
|
|---|
| 372 | len = 512 - byte_offset;
|
|---|
| 373 | if (len > byte_len)
|
|---|
| 374 | len = byte_len;
|
|---|
| 375 | memcpy(dest, sector_buffer + byte_offset, len);
|
|---|
| 376 | sector++;
|
|---|
| 377 | byte_offset = 0;
|
|---|
| 378 | byte_len -= len;
|
|---|
| 379 | dest += len;
|
|---|
| 380 | }
|
|---|
| 381 |
|
|---|
| 382 | return 1;
|
|---|
| 383 | }
|
|---|
