MBRBlockDevice.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2017 ARM Limited
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "MBRBlockDevice.h"
00018 #include <algorithm>
00019 
00020 
00021 // On disk structures, all entries are little endian
00022 MBED_PACKED(struct) mbr_entry {
00023     uint8_t status;
00024     uint8_t chs_start[3];
00025     uint8_t type;
00026     uint8_t chs_stop[3];
00027     uint32_t lba_offset;
00028     uint32_t lba_size;
00029 };
00030 
00031 MBED_PACKED(struct) mbr_table {
00032     struct mbr_entry entries[4];
00033     uint8_t signature[2];
00034 };
00035 
00036 // Little-endian conversion, should compile to noop
00037 // if system is little-endian
00038 static inline uint32_t tole32(uint32_t a)
00039 {
00040     union {
00041         uint32_t u32;
00042         uint8_t u8[4];
00043     } w;
00044 
00045     w.u8[0] = a >>  0;
00046     w.u8[1] = a >>  8;
00047     w.u8[2] = a >> 16;
00048     w.u8[3] = a >> 24;
00049 
00050     return w.u32;
00051 }
00052 
00053 static inline uint32_t fromle32(uint32_t a)
00054 {
00055     return tole32(a);
00056 }
00057 
00058 static void tochs(uint32_t lba, uint8_t chs[3])
00059 {
00060     uint32_t sector = std::min<uint32_t>(lba, 0xfffffd)+1;
00061     chs[0] = (sector >> 6) & 0xff;
00062     chs[1] = ((sector >> 0) & 0x3f) | ((sector >> 16) & 0xc0);
00063     chs[2] = (sector >> 14) & 0xff;
00064 }
00065 
00066 
00067 // Partition after address are turned into absolute
00068 // addresses, assumes bd is initialized
00069 static int partition_absolute(
00070         BlockDevice *bd, int part, uint8_t type,
00071         bd_size_t offset, bd_size_t size)
00072 {
00073     // Allocate smallest buffer necessary to write MBR
00074     uint32_t buffer_size = std::max<uint32_t>(bd->get_program_size(), sizeof(struct mbr_table));
00075     uint8_t *buffer = new uint8_t[buffer_size];
00076 
00077     // Check for existing MBR
00078     int err = bd->read(buffer, 512-buffer_size, buffer_size);
00079     if (err) {
00080         delete[] buffer;
00081         return err;
00082     }
00083 
00084     struct mbr_table *table = reinterpret_cast<struct mbr_table*>(
00085             &buffer[buffer_size - sizeof(struct mbr_table)]);
00086     if (table->signature[0] != 0x55 || table->signature[1] != 0xaa) {
00087         // Setup default values for MBR
00088         table->signature[0] = 0x55;
00089         table->signature[1] = 0xaa;
00090         memset(table->entries, 0, sizeof(table->entries));
00091     }
00092 
00093     // Setup new partition
00094     MBED_ASSERT(part >= 1 && part <= 4);
00095     table->entries[part-1].status = 0x00; // inactive (not bootable)
00096     table->entries[part-1].type = type;
00097 
00098     // lba dimensions
00099     uint32_t sector = std::max<uint32_t>(bd->get_erase_size(), 512);
00100     uint32_t lba_offset = offset / sector;
00101     uint32_t lba_size = size / sector;
00102     table->entries[part-1].lba_offset = tole32(lba_offset);
00103     table->entries[part-1].lba_size = tole32(lba_size);
00104 
00105     // chs dimensions
00106     tochs(lba_offset,            table->entries[part-1].chs_start);
00107     tochs(lba_offset+lba_size-1, table->entries[part-1].chs_stop);
00108 
00109     // Write out MBR
00110     err = bd->erase(0, bd->get_erase_size());
00111     if (err) {
00112         delete[] buffer;
00113         return err;
00114     }
00115 
00116     err = bd->program(buffer, 512-buffer_size, buffer_size);
00117     delete[] buffer;
00118     return err;
00119 }
00120 
00121 int MBRBlockDevice::partition(BlockDevice *bd, int part, uint8_t type, bd_addr_t start)
00122 {
00123     int err = bd->init();
00124     if (err) {
00125         return err;
00126     }
00127 
00128     // Calculate dimensions
00129     bd_size_t offset = ((int64_t)start < 0) ? -start : start;
00130     bd_size_t size = bd->size();
00131 
00132     if (offset < 512) {
00133         offset += std::max<uint32_t>(bd->get_erase_size(), 512);
00134     }
00135 
00136     size -= offset;
00137 
00138     err = partition_absolute(bd, part, type, offset, size);
00139     if (err) {
00140         return err;
00141     }
00142 
00143     err = bd->deinit();
00144     if (err) {
00145         return err;
00146     }
00147 
00148     return 0;
00149 }
00150 
00151 int MBRBlockDevice::partition(BlockDevice *bd, int part, uint8_t type,
00152         bd_addr_t start, bd_addr_t stop)
00153 {
00154     int err = bd->init();
00155     if (err) {
00156         return err;
00157     }
00158 
00159     // Calculate dimensions
00160     bd_size_t offset = ((int64_t)start < 0) ? -start : start;
00161     bd_size_t size = ((int64_t)stop < 0) ? -stop : stop;
00162 
00163     if (offset < 512) {
00164         offset += std::max<uint32_t>(bd->get_erase_size(), 512);
00165     }
00166 
00167     size -= offset;
00168 
00169     err = partition_absolute(bd, part, type, offset, size);
00170     if (err) {
00171         return err;
00172     }
00173 
00174     err = bd->deinit();
00175     if (err) {
00176         return err;
00177     }
00178 
00179     return 0;
00180 }
00181 
00182 MBRBlockDevice::MBRBlockDevice(BlockDevice *bd, int part)
00183     : _bd(bd), _part(part)
00184 {
00185     MBED_ASSERT(_part >= 1 && _part <= 4);
00186 }
00187 
00188 int MBRBlockDevice::init()
00189 {
00190     int err = _bd->init();
00191     if (err) {
00192         return err;
00193     }
00194 
00195     // Allocate smallest buffer necessary to write MBR
00196     uint32_t buffer_size = std::max<uint32_t>(_bd->get_read_size(), sizeof(struct mbr_table));
00197     uint8_t *buffer = new uint8_t[buffer_size];
00198 
00199     err = _bd->read(buffer, 512-buffer_size, buffer_size);
00200     if (err) {
00201         delete[] buffer;
00202         return err;
00203     }
00204 
00205     // Check for valid table
00206     struct mbr_table *table = reinterpret_cast<struct mbr_table*>(
00207             &buffer[buffer_size - sizeof(struct mbr_table)]);
00208     if (table->signature[0] != 0x55 || table->signature[1] != 0xaa) {
00209         delete[] buffer;
00210         return BD_ERROR_INVALID_MBR;
00211     }
00212 
00213     // Check for valid entry
00214     if (table->entries[_part-1].type == 0x00) {
00215         delete[] buffer;
00216         return BD_ERROR_INVALID_PARTITION;
00217     }
00218 
00219     // Get partition attributes
00220     bd_size_t sector = std::max<uint32_t>(_bd->get_erase_size(), 512);
00221     _type = table->entries[_part-1].type;
00222     _offset = fromle32(table->entries[_part-1].lba_offset) * sector;
00223     _size   = fromle32(table->entries[_part-1].lba_size)   * sector;
00224 
00225     // Check that block addresses are valid
00226     MBED_ASSERT(_bd->is_valid_erase(_offset, _size));
00227 
00228     delete[] buffer;
00229     return 0;
00230 }
00231 
00232 int MBRBlockDevice::deinit()
00233 {
00234     return _bd->deinit();
00235 }
00236 
00237 int MBRBlockDevice::read(void *b, bd_addr_t addr, bd_size_t size)
00238 {
00239     MBED_ASSERT(is_valid_read(addr, size));
00240     return _bd->read(b, addr + _offset, size);
00241 }
00242 
00243 int MBRBlockDevice::program(const void *b, bd_addr_t addr, bd_size_t size)
00244 {
00245     MBED_ASSERT(is_valid_program(addr, size));
00246     return _bd->program(b, addr + _offset, size);
00247 }
00248 
00249 int MBRBlockDevice::erase(bd_addr_t addr, bd_size_t size)
00250 {
00251     MBED_ASSERT(is_valid_erase(addr, size));
00252     return _bd->erase(addr + _offset, size);
00253 }
00254 
00255 bd_size_t MBRBlockDevice::get_read_size() const
00256 {
00257     return _bd->get_read_size();
00258 }
00259 
00260 bd_size_t MBRBlockDevice::get_program_size() const
00261 {
00262     return _bd->get_program_size();
00263 }
00264 
00265 bd_size_t MBRBlockDevice::get_erase_size() const
00266 {
00267     return _bd->get_erase_size();
00268 }
00269 
00270 bd_size_t MBRBlockDevice::size() const
00271 {
00272     return _size;
00273 }
00274 
00275 bd_size_t MBRBlockDevice::get_partition_start() const
00276 {
00277     return _offset;
00278 }
00279 
00280 bd_size_t MBRBlockDevice::get_partition_stop() const
00281 {
00282     return _offset+_size;
00283 }
00284 
00285 uint8_t MBRBlockDevice::get_partition_type() const
00286 {
00287     return _type;
00288 }
00289 
00290 int MBRBlockDevice::get_partition_number() const
00291 {
00292     return _part;
00293 }