Marco Zecchini
/
Example_RTOS
Rtos API example
Embed:
(wiki syntax)
Show/hide line numbers
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 }
Generated on Sun Jul 17 2022 08:25:28 by 1.7.2