takashi kadono / Mbed OS Nucleo_446

Dependencies:   ssd1331

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 "mbed_critical.h"
00019 #include <algorithm>
00020 
00021 
00022 // On disk structures, all entries are little endian
00023 MBED_PACKED(struct) mbr_entry {
00024     uint8_t status;
00025     uint8_t chs_start[3];
00026     uint8_t type;
00027     uint8_t chs_stop[3];
00028     uint32_t lba_offset;
00029     uint32_t lba_size;
00030 };
00031 
00032 MBED_PACKED(struct) mbr_table {
00033     struct mbr_entry entries[4];
00034     uint8_t signature[2];
00035 };
00036 
00037 // Little-endian conversion, should compile to noop
00038 // if system is little-endian
00039 static inline uint32_t tole32(uint32_t a)
00040 {
00041     union {
00042         uint32_t u32;
00043         uint8_t u8[4];
00044     } w;
00045 
00046     w.u8[0] = a >>  0;
00047     w.u8[1] = a >>  8;
00048     w.u8[2] = a >> 16;
00049     w.u8[3] = a >> 24;
00050 
00051     return w.u32;
00052 }
00053 
00054 static inline uint32_t fromle32(uint32_t a)
00055 {
00056     return tole32(a);
00057 }
00058 
00059 static void tochs(uint32_t lba, uint8_t chs[3])
00060 {
00061     uint32_t sector = std::min<uint32_t>(lba, 0xfffffd)+1;
00062     chs[0] = (sector >> 6) & 0xff;
00063     chs[1] = ((sector >> 0) & 0x3f) | ((sector >> 16) & 0xc0);
00064     chs[2] = (sector >> 14) & 0xff;
00065 }
00066 
00067 
00068 // Partition after address are turned into absolute
00069 // addresses, assumes bd is initialized
00070 static int partition_absolute(
00071         BlockDevice *bd, int part, uint8_t type,
00072         bd_size_t offset, bd_size_t size)
00073 {
00074     // Allocate smallest buffer necessary to write MBR
00075     uint32_t buffer_size = std::max<uint32_t>(bd->get_program_size(), sizeof(struct mbr_table));
00076 
00077     // Prevent alignment issues
00078     if(buffer_size % bd->get_program_size() != 0) {
00079         buffer_size += bd->get_program_size() - (buffer_size % bd->get_program_size());
00080     }
00081 
00082     uint8_t *buffer = new uint8_t[buffer_size];
00083 
00084     // Check for existing MBR
00085     int err = bd->read(buffer, 512-buffer_size, buffer_size);
00086     if (err) {
00087         delete[] buffer;
00088         return err;
00089     }
00090 
00091     struct mbr_table *table = reinterpret_cast<struct mbr_table*>(
00092             &buffer[buffer_size - sizeof(struct mbr_table)]);
00093     if (table->signature[0] != 0x55 || table->signature[1] != 0xaa) {
00094         // Setup default values for MBR
00095         table->signature[0] = 0x55;
00096         table->signature[1] = 0xaa;
00097         memset(table->entries, 0, sizeof(table->entries));
00098     }
00099 
00100     // Setup new partition
00101     MBED_ASSERT(part >= 1 && part <= 4);
00102     table->entries[part-1].status = 0x00; // inactive (not bootable)
00103     table->entries[part-1].type = type;
00104 
00105     // lba dimensions
00106     MBED_ASSERT(bd->is_valid_erase(offset, size));
00107     uint32_t sector = std::max<uint32_t>(bd->get_erase_size(), 512);
00108     uint32_t lba_offset = offset / sector;
00109     uint32_t lba_size = size / sector;
00110     table->entries[part-1].lba_offset = tole32(lba_offset);
00111     table->entries[part-1].lba_size = tole32(lba_size);
00112 
00113     // chs dimensions
00114     tochs(lba_offset,            table->entries[part-1].chs_start);
00115     tochs(lba_offset+lba_size-1, table->entries[part-1].chs_stop);
00116 
00117     // Check that we don't overlap other entries
00118     for (int i = 1; i <= 4; i++) {
00119         if (i != part && table->entries[i-1].type != 0x00) {
00120             uint32_t neighbor_lba_offset = fromle32(table->entries[i-1].lba_offset);
00121             uint32_t neighbor_lba_size = fromle32(table->entries[i-1].lba_size);
00122             MBED_ASSERT(
00123                     (lba_offset >= neighbor_lba_offset + neighbor_lba_size) ||
00124                     (lba_offset + lba_size <= neighbor_lba_offset));
00125             (void)neighbor_lba_offset;
00126             (void)neighbor_lba_size;
00127         }
00128     }
00129 
00130     // Write out MBR
00131     err = bd->erase(0, bd->get_erase_size());
00132     if (err) {
00133         delete[] buffer;
00134         return err;
00135     }
00136 
00137     err = bd->program(buffer, 512-buffer_size, buffer_size);
00138     delete[] buffer;
00139     return err;
00140 }
00141 
00142 int MBRBlockDevice::partition(BlockDevice *bd, int part, uint8_t type, bd_addr_t start)
00143 {
00144     int err = bd->init();
00145     if (err) {
00146         return err;
00147     }
00148 
00149     // Calculate dimensions
00150     bd_size_t offset = ((int64_t)start < 0) ? -start : start;
00151     bd_size_t size = bd->size();
00152 
00153     if (offset < 512) {
00154         offset += std::max<uint32_t>(bd->get_erase_size(), 512);
00155     }
00156 
00157     size -= offset;
00158 
00159     err = partition_absolute(bd, part, type, offset, size);
00160     if (err) {
00161         return err;
00162     }
00163 
00164     err = bd->deinit();
00165     if (err) {
00166         return err;
00167     }
00168 
00169     return 0;
00170 }
00171 
00172 int MBRBlockDevice::partition(BlockDevice *bd, int part, uint8_t type,
00173         bd_addr_t start, bd_addr_t stop)
00174 {
00175     int err = bd->init();
00176     if (err) {
00177         return err;
00178     }
00179 
00180     // Calculate dimensions
00181     bd_size_t offset = ((int64_t)start < 0) ? -start : start;
00182     bd_size_t size = ((int64_t)stop < 0) ? -stop : stop;
00183 
00184     if (offset < 512) {
00185         offset += std::max<uint32_t>(bd->get_erase_size(), 512);
00186     }
00187 
00188     size -= offset;
00189 
00190     err = partition_absolute(bd, part, type, offset, size);
00191     if (err) {
00192         return err;
00193     }
00194 
00195     err = bd->deinit();
00196     if (err) {
00197         return err;
00198     }
00199 
00200     return 0;
00201 }
00202 
00203 MBRBlockDevice::MBRBlockDevice(BlockDevice *bd, int part)
00204     : _bd(bd), _offset(0), _size(0), _type(0), _part(part), _init_ref_count(0), _is_initialized(false)
00205 {
00206     MBED_ASSERT(_part >= 1 && _part <= 4);
00207 }
00208 
00209 int MBRBlockDevice::init()
00210 {
00211     uint32_t buffer_size;
00212     uint8_t *buffer = 0;
00213     struct mbr_table *table;
00214     bd_size_t sector;
00215     int err;
00216 
00217     uint32_t val = core_util_atomic_incr_u32(&_init_ref_count, 1);
00218 
00219     if (val != 1) {
00220         return BD_ERROR_OK;
00221     }
00222 
00223     err = _bd->init();
00224     if (err) {
00225         goto fail;
00226     }
00227 
00228     // Allocate smallest buffer necessary to write MBR
00229     buffer_size = std::max<uint32_t>(_bd->get_read_size(), sizeof(struct mbr_table));
00230     buffer = new uint8_t[buffer_size];
00231 
00232     err = _bd->read(buffer, 512-buffer_size, buffer_size);
00233     if (err) {
00234         goto fail;
00235     }
00236 
00237     // Check for valid table
00238     table = reinterpret_cast<struct mbr_table*>(&buffer[buffer_size - sizeof(struct mbr_table)]);
00239     if (table->signature[0] != 0x55 || table->signature[1] != 0xaa) {
00240         err = BD_ERROR_INVALID_MBR;
00241         goto fail;
00242     }
00243 
00244     // Check for valid entry
00245     // 0x00 = no entry
00246     // 0x05, 0x0f = extended partitions, currently not supported
00247     if ((table->entries[_part-1].type == 0x00 ||
00248          table->entries[_part-1].type == 0x05 ||
00249          table->entries[_part-1].type == 0x0f)) {
00250         err = BD_ERROR_INVALID_PARTITION;
00251         goto fail;
00252     }
00253 
00254     // Get partition attributes
00255     sector = std::max<uint32_t>(_bd->get_erase_size(), 512);
00256     _type = table->entries[_part-1].type;
00257     _offset = fromle32(table->entries[_part-1].lba_offset) * sector;
00258     _size   = fromle32(table->entries[_part-1].lba_size)   * sector;
00259 
00260     // Check that block addresses are valid
00261     if (!_bd->is_valid_erase(_offset, _size)) {
00262         err = BD_ERROR_INVALID_PARTITION;
00263         goto fail;
00264     }
00265 
00266     _is_initialized = true;
00267     delete[] buffer;
00268     return BD_ERROR_OK;
00269 
00270 fail:
00271     delete[] buffer;
00272     _is_initialized = false;
00273     _init_ref_count = 0;
00274     return err;
00275 }
00276 
00277 int MBRBlockDevice::deinit()
00278 {
00279     if (!_is_initialized) {
00280         return BD_ERROR_OK;
00281     }
00282 
00283     uint32_t val = core_util_atomic_decr_u32(&_init_ref_count, 1);
00284 
00285     if (val) {
00286         return BD_ERROR_OK;
00287     }
00288 
00289     _is_initialized = false;
00290     return _bd->deinit();
00291 }
00292 
00293 int MBRBlockDevice::sync()
00294 {
00295     if (!_is_initialized) {
00296         return BD_ERROR_DEVICE_ERROR;
00297     }
00298 
00299     return _bd->sync();
00300 }
00301 
00302 int MBRBlockDevice::read(void *b, bd_addr_t addr, bd_size_t size)
00303 {
00304     MBED_ASSERT(is_valid_read(addr, size));
00305     if (!_is_initialized) {
00306         return BD_ERROR_DEVICE_ERROR;
00307     }
00308 
00309     return _bd->read(b, addr + _offset, size);
00310 }
00311 
00312 int MBRBlockDevice::program(const void *b, bd_addr_t addr, bd_size_t size)
00313 {
00314     MBED_ASSERT(is_valid_program(addr, size));
00315     if (!_is_initialized) {
00316         return BD_ERROR_DEVICE_ERROR;
00317     }
00318 
00319     return _bd->program(b, addr + _offset, size);
00320 }
00321 
00322 int MBRBlockDevice::erase(bd_addr_t addr, bd_size_t size)
00323 {
00324     MBED_ASSERT(is_valid_erase(addr, size));
00325     if (!_is_initialized) {
00326         return BD_ERROR_DEVICE_ERROR;
00327     }
00328 
00329     return _bd->erase(addr + _offset, size);
00330 }
00331 
00332 bd_size_t MBRBlockDevice::get_read_size() const
00333 {
00334     if (!_is_initialized) {
00335         return 0;
00336     }
00337 
00338     return _bd->get_read_size();
00339 }
00340 
00341 bd_size_t MBRBlockDevice::get_program_size() const
00342 {
00343     if (!_is_initialized) {
00344         return 0;
00345     }
00346 
00347     return _bd->get_program_size();
00348 }
00349 
00350 bd_size_t MBRBlockDevice::get_erase_size() const
00351 {
00352     if (!_is_initialized) {
00353         return 0;
00354     }
00355 
00356     return _bd->get_erase_size();
00357 }
00358 
00359 bd_size_t MBRBlockDevice::get_erase_size(bd_addr_t addr) const
00360 {
00361     if (!_is_initialized) {
00362         return 0;
00363     }
00364 
00365     return _bd->get_erase_size(_offset + addr);
00366 }
00367 
00368 int MBRBlockDevice::get_erase_value() const
00369 {
00370     if (!_is_initialized) {
00371         return 0;
00372     }
00373 
00374     return _bd->get_erase_value();
00375 }
00376 
00377 bd_size_t MBRBlockDevice::size() const
00378 {
00379     return _size;
00380 }
00381 
00382 bd_size_t MBRBlockDevice::get_partition_start() const
00383 {
00384     return _offset;
00385 }
00386 
00387 bd_size_t MBRBlockDevice::get_partition_stop() const
00388 {
00389     return _offset+_size;
00390 }
00391 
00392 uint8_t MBRBlockDevice::get_partition_type() const
00393 {
00394     return _type;
00395 }
00396 
00397 int MBRBlockDevice::get_partition_number() const
00398 {
00399     return _part;
00400 }