Knight KE / Mbed OS Game_Master
Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MBRBlockDevice.cpp Source File

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     MBED_ASSERT(bd->is_valid_erase(offset, size));
00100     uint32_t sector = std::max<uint32_t>(bd->get_erase_size(), 512);
00101     uint32_t lba_offset = offset / sector;
00102     uint32_t lba_size = size / sector;
00103     table->entries[part-1].lba_offset = tole32(lba_offset);
00104     table->entries[part-1].lba_size = tole32(lba_size);
00105 
00106     // chs dimensions
00107     tochs(lba_offset,            table->entries[part-1].chs_start);
00108     tochs(lba_offset+lba_size-1, table->entries[part-1].chs_stop);
00109 
00110     // Check that we don't overlap other entries
00111     for (int i = 1; i <= 4; i++) {
00112         if (i != part && table->entries[i-1].type != 0x00) {
00113             uint32_t neighbor_lba_offset = fromle32(table->entries[i-1].lba_offset);
00114             uint32_t neighbor_lba_size = fromle32(table->entries[i-1].lba_size);
00115             MBED_ASSERT(
00116                     (lba_offset >= neighbor_lba_offset + neighbor_lba_size) ||
00117                     (lba_offset + lba_size <= neighbor_lba_offset));
00118             (void)neighbor_lba_offset;
00119             (void)neighbor_lba_size;
00120         }
00121     }
00122 
00123     // Write out MBR
00124     err = bd->erase(0, bd->get_erase_size());
00125     if (err) {
00126         delete[] buffer;
00127         return err;
00128     }
00129 
00130     err = bd->program(buffer, 512-buffer_size, buffer_size);
00131     delete[] buffer;
00132     return err;
00133 }
00134 
00135 int MBRBlockDevice::partition(BlockDevice *bd, int part, uint8_t type, bd_addr_t start)
00136 {
00137     int err = bd->init();
00138     if (err) {
00139         return err;
00140     }
00141 
00142     // Calculate dimensions
00143     bd_size_t offset = ((int64_t)start < 0) ? -start : start;
00144     bd_size_t size = bd->size();
00145 
00146     if (offset < 512) {
00147         offset += std::max<uint32_t>(bd->get_erase_size(), 512);
00148     }
00149 
00150     size -= offset;
00151 
00152     err = partition_absolute(bd, part, type, offset, size);
00153     if (err) {
00154         return err;
00155     }
00156 
00157     err = bd->deinit();
00158     if (err) {
00159         return err;
00160     }
00161 
00162     return 0;
00163 }
00164 
00165 int MBRBlockDevice::partition(BlockDevice *bd, int part, uint8_t type,
00166         bd_addr_t start, bd_addr_t stop)
00167 {
00168     int err = bd->init();
00169     if (err) {
00170         return err;
00171     }
00172 
00173     // Calculate dimensions
00174     bd_size_t offset = ((int64_t)start < 0) ? -start : start;
00175     bd_size_t size = ((int64_t)stop < 0) ? -stop : stop;
00176 
00177     if (offset < 512) {
00178         offset += std::max<uint32_t>(bd->get_erase_size(), 512);
00179     }
00180 
00181     size -= offset;
00182 
00183     err = partition_absolute(bd, part, type, offset, size);
00184     if (err) {
00185         return err;
00186     }
00187 
00188     err = bd->deinit();
00189     if (err) {
00190         return err;
00191     }
00192 
00193     return 0;
00194 }
00195 
00196 MBRBlockDevice::MBRBlockDevice(BlockDevice *bd, int part)
00197     : _bd(bd), _part(part)
00198 {
00199     MBED_ASSERT(_part >= 1 && _part <= 4);
00200 }
00201 
00202 int MBRBlockDevice::init()
00203 {
00204     int err = _bd->init();
00205     if (err) {
00206         return err;
00207     }
00208 
00209     // Allocate smallest buffer necessary to write MBR
00210     uint32_t buffer_size = std::max<uint32_t>(_bd->get_read_size(), sizeof(struct mbr_table));
00211     uint8_t *buffer = new uint8_t[buffer_size];
00212 
00213     err = _bd->read(buffer, 512-buffer_size, buffer_size);
00214     if (err) {
00215         delete[] buffer;
00216         return err;
00217     }
00218 
00219     // Check for valid table
00220     struct mbr_table *table = reinterpret_cast<struct mbr_table*>(
00221             &buffer[buffer_size - sizeof(struct mbr_table)]);
00222     if (table->signature[0] != 0x55 || table->signature[1] != 0xaa) {
00223         delete[] buffer;
00224         return BD_ERROR_INVALID_MBR;
00225     }
00226 
00227     // Check for valid entry
00228     // 0x00 = no entry
00229     // 0x05, 0x0f = extended partitions, currently not supported
00230     if ((table->entries[_part-1].type == 0x00 ||
00231          table->entries[_part-1].type == 0x05 ||
00232          table->entries[_part-1].type == 0x0f)) {
00233         delete[] buffer;
00234         return BD_ERROR_INVALID_PARTITION;
00235     }
00236 
00237     // Get partition attributes
00238     bd_size_t sector = std::max<uint32_t>(_bd->get_erase_size(), 512);
00239     _type = table->entries[_part-1].type;
00240     _offset = fromle32(table->entries[_part-1].lba_offset) * sector;
00241     _size   = fromle32(table->entries[_part-1].lba_size)   * sector;
00242 
00243     // Check that block addresses are valid
00244     MBED_ASSERT(_bd->is_valid_erase(_offset, _size));
00245 
00246     delete[] buffer;
00247     return 0;
00248 }
00249 
00250 int MBRBlockDevice::deinit()
00251 {
00252     return _bd->deinit();
00253 }
00254 
00255 int MBRBlockDevice::sync()
00256 {
00257     return _bd->sync();
00258 }
00259 
00260 int MBRBlockDevice::read(void *b, bd_addr_t addr, bd_size_t size)
00261 {
00262     MBED_ASSERT(is_valid_read(addr, size));
00263     return _bd->read(b, addr + _offset, size);
00264 }
00265 
00266 int MBRBlockDevice::program(const void *b, bd_addr_t addr, bd_size_t size)
00267 {
00268     MBED_ASSERT(is_valid_program(addr, size));
00269     return _bd->program(b, addr + _offset, size);
00270 }
00271 
00272 int MBRBlockDevice::erase(bd_addr_t addr, bd_size_t size)
00273 {
00274     MBED_ASSERT(is_valid_erase(addr, size));
00275     return _bd->erase(addr + _offset, size);
00276 }
00277 
00278 bd_size_t MBRBlockDevice::get_read_size() const
00279 {
00280     return _bd->get_read_size();
00281 }
00282 
00283 bd_size_t MBRBlockDevice::get_program_size() const
00284 {
00285     return _bd->get_program_size();
00286 }
00287 
00288 bd_size_t MBRBlockDevice::get_erase_size() const
00289 {
00290     return _bd->get_erase_size();
00291 }
00292 
00293 bd_size_t MBRBlockDevice::get_erase_size(bd_addr_t addr) const
00294 {
00295     return _bd->get_erase_size(_offset + addr);
00296 }
00297 
00298 int MBRBlockDevice::get_erase_value() const
00299 {
00300     return _bd->get_erase_value();
00301 }
00302 
00303 bd_size_t MBRBlockDevice::size() const
00304 {
00305     return _size;
00306 }
00307 
00308 bd_size_t MBRBlockDevice::get_partition_start() const
00309 {
00310     return _offset;
00311 }
00312 
00313 bd_size_t MBRBlockDevice::get_partition_stop() const
00314 {
00315     return _offset+_size;
00316 }
00317 
00318 uint8_t MBRBlockDevice::get_partition_type() const
00319 {
00320     return _type;
00321 }
00322 
00323 int MBRBlockDevice::get_partition_number() const
00324 {
00325     return _part;
00326 }