SOF_block.cpp

00001 /*
00002 * @file SOF_block.cpp
00003 *
00004 * @brief MTD device handling of STM32 internal flash memory.
00005 *
00006 *
00007 * History:
00008 */
00009 
00010 #include <stdio.h>
00011 #include <assert.h>
00012 #include "SOF_dev.h"
00013 #include <string.h>
00014 
00015 #define LOCAL_DEBUG 0 // turn on local debug
00016 
00017 #define DCRLF   "\r\n"
00018 
00019 #if LOCAL_DEBUG
00020 #define DPRINTF printf
00021 #define EPRINTF printf
00022 #define DUMP_BLOCK dump_block
00023 #define DASSERT assert
00024 #else
00025 #define DPRINTF(...)
00026 #define EPRINTF(...)
00027 #define DUMP_BLOCK(...)
00028 #define DASSERT(...)
00029 #endif
00030 
00031 static uint16_t checksum(const uint8_t* data, int count);
00032 
00033 #define SYNC_MARK_BYTE_IN_LEN   0x07    // signature mark for upper byte in the storage_len
00034 
00035 #define RESERVED_BLOCK_INFO_SIZE sizeof(BlockInfo_t)
00036 
00037 typedef struct {
00038     uint16_t for_storage;
00039     uint16_t for_info;
00040 } BlockChecksum_t;
00041 
00042 typedef struct {
00043     BlockChecksum_t     csum;
00044     uint32_t            storage_len;
00045 } BlockInfo_t;
00046 
00047 typedef struct {
00048     uint32_t    begin_offset;
00049     uint32_t    len;
00050     uint16_t    storage_csum;
00051 } StorageInfo_t;
00052 
00053 class SOF_BlockHandle
00054 {
00055 public:
00056     SOF_BlockHandle()
00057         : write_mode_(false)
00058         , cur_pos_(0)
00059         , hdev_(SOF_INVALID_HANDLE)
00060         , storage_max_offset_(0)
00061         , storage_begin_offset_(0)
00062         , storage_end_offset_(0) {
00063     }
00064 
00065     bool is_writable() const {
00066         return write_mode_;
00067     }
00068 
00069     uint32_t total_physical_block_size() const {
00070         return SOF_dev_info(hdev_)->sec_size;
00071     }
00072 
00073 public:
00074     bool                write_mode_;
00075     size_t              cur_pos_;
00076     SOF_DevHandle_t     hdev_;
00077     uint32_t            storage_max_offset_;
00078     uint32_t            storage_begin_offset_;
00079     uint32_t            storage_end_offset_;
00080 };
00081 
00082 
00083 static bool get_block_info(const SOF_BlockHandle_t handle, size_t seq, BlockInfo_t *info, uint32_t *loc_offset)
00084 {
00085     uint32_t check_pos = ((seq+1) * sizeof(BlockInfo_t));
00086     uint32_t info_pos;
00087 
00088     DASSERT(check_pos < handle->total_physical_block_size());
00089     if (check_pos >= handle->total_physical_block_size())
00090         return false;
00091 
00092     *loc_offset = info_pos = handle->total_physical_block_size() - check_pos;
00093 
00094     // checksum in the first word
00095     *((uint32_t*)&info->csum) = SOF_dev_read_word(handle->hdev_, info_pos);
00096     // storage len in the next word
00097     info->storage_len = SOF_dev_read_word(handle->hdev_, info_pos + 4);
00098 
00099     return true;
00100 }
00101 
00102 static bool is_empty_block_info(BlockInfo_t *info)
00103 {
00104     uint8_t *p = (uint8_t*)info;
00105 
00106     for (size_t i = 0; i < sizeof(BlockInfo_t); ++i)
00107         if (p[i] != SOF_ERASED_BYTE_VALUE)
00108             return false;
00109 
00110     return true;
00111 }
00112 
00113 static bool is_valid_block_info(BlockInfo_t *info)
00114 {
00115     uint16_t csum = checksum((uint8_t*)&info->storage_len, 4);
00116 
00117     if (SYNC_MARK_BYTE_IN_LEN != (info->storage_len >> 24)) {
00118         EPRINTF("no sync mark in storage_len=%#x"DCRLF,info->storage_len);
00119         return false;
00120     }
00121 
00122     if (csum != info->csum.for_info) {
00123         EPRINTF("CSUM mismatch %#x %#x"DCRLF,csum, info->csum.for_info);
00124         return false;
00125     }
00126 
00127     return true;
00128 }
00129 
00130 static bool get_empty_info_location(const SOF_BlockHandle_t handle, uint32_t *loc_offset)
00131 {
00132     BlockInfo_t info;
00133     uint32_t pos;
00134 
00135     for (size_t seq = 0; get_block_info(handle, seq, &info, &pos); ++seq) {
00136         //DPRINTF("[%u] len=%#x pos=%u"DCRLF,seq, info.storage_len, pos);
00137         if (is_empty_block_info(&info)) {
00138             *loc_offset = pos;
00139             return true;
00140         }
00141     }
00142 
00143     return false;
00144 }
00145 
00146 static SOF_Error_t probe_active_storage_info(const SOF_BlockHandle_t handle, StorageInfo_t *storage_info)
00147 {
00148     BlockInfo_t info, last_info;
00149     uint32_t pos;
00150     uint32_t storage_len_sum = 0;
00151 
00152     for (size_t seq = 0; get_block_info(handle, seq, &info, &pos); ++seq) {
00153         if (is_empty_block_info(&info)) {
00154             if (seq == 0)
00155                 return kSOF_ErrNoInfo;
00156             break;
00157         }
00158         if (!is_valid_block_info(&info)) {
00159             if (storage_info->begin_offset + storage_info->len == pos) {
00160                 DPRINTF("data is full: %u"DCRLF,storage_info->begin_offset + storage_info->len);
00161                 break;
00162             }
00163 
00164             EPRINTF("invalid block at %u"DCRLF,pos);
00165             return kSOF_ErrBadBlock;
00166         }
00167 
00168         storage_len_sum += info.storage_len & 0x00FFFFFF;
00169         last_info = info;
00170     }
00171 
00172     uint32_t storage_len = last_info.storage_len & 0x00FFFFFF;
00173 
00174     storage_info->begin_offset = storage_len_sum - storage_len;
00175     storage_info->len = storage_len;
00176     storage_info->storage_csum = last_info.csum.for_storage;
00177 
00178     return kSOF_ErrNone;
00179 }
00180 
00181 
00182 #if LOCAL_DEBUG
00183 static void dump_block(SOF_BlockHandle_t handle)
00184 {
00185     DPRINTF("sector(%u)"DCRLF, SOF_dev_info(handle->hdev_)->sec_no);
00186     DPRINTF("   offset               =%u"DCRLF, handle->cur_pos_);
00187     DPRINTF("   writemode            =%d"DCRLF, handle->write_mode_);
00188     DPRINTF("   storage_max_offset   =%u"DCRLF, handle->storage_max_offset_);
00189     DPRINTF("   storage_begin_offset =%u"DCRLF, handle->storage_begin_offset_);
00190     DPRINTF("   storage_end_offset   =%u"DCRLF, handle->storage_end_offset_);
00191     DPRINTF("   free=%u total=%u"DCRLF,SOF_block_get_free_size(handle), handle->total_physical_block_size());
00192 }
00193 #endif
00194 
00195 size_t SOF_block_get_free_size(SOF_BlockHandle_t handle)
00196 {
00197     DASSERT(handle != NULL);
00198     if (handle->storage_end_offset_ <= handle->storage_max_offset_-RESERVED_BLOCK_INFO_SIZE)
00199         return (handle->storage_max_offset_- RESERVED_BLOCK_INFO_SIZE) - handle->storage_end_offset_;
00200     else {
00201         return 0;
00202     }
00203 }
00204 
00205 uint32_t SOF_block_storage_size(SOF_BlockHandle_t handle)
00206 {
00207     DASSERT(handle != NULL);
00208     return handle->storage_end_offset_ - handle->storage_begin_offset_;
00209 }
00210 
00211 static uint16_t checksum(const uint8_t* data, int count)
00212 {
00213     // Fletcher's checksum algorithm
00214     uint16_t sum1 = 0;
00215     uint16_t sum2 = 0;
00216     int index;
00217 
00218     for( index = 0; index < count; ++index ) {
00219         sum1 = (sum1 + data[index]) % 255;
00220         sum2 = (sum2 + sum1) % 255;
00221     }
00222 
00223     return (sum2 << 8) | sum1;
00224 }
00225 
00226 static uint16_t compute_storage_checksum(SOF_BlockHandle_t handle)
00227 {
00228     uint8_t *addr = SOF_dev_get_hw_addr(handle->hdev_);
00229 
00230     return checksum(addr+handle->storage_begin_offset_, SOF_block_storage_size(handle));
00231 }
00232 
00233 static bool write_storage_info(SOF_BlockHandle_t handle)
00234 {
00235     BlockInfo_t cs;
00236 
00237     cs.storage_len = (SYNC_MARK_BYTE_IN_LEN << 24) | SOF_block_storage_size(handle);
00238     cs.csum.for_info = checksum((uint8_t*)&cs.storage_len, 4);
00239     cs.csum.for_storage = compute_storage_checksum(handle);
00240 
00241     DPRINTF("write %#x at %#x"DCRLF,*((uint32_t*)&cs.csum), handle->storage_max_offset_);
00242     if (SOF_dev_write_word(handle->hdev_, handle->storage_max_offset_, *((uint32_t*)&cs.csum)) < 0)
00243         return false;
00244 
00245     if (SOF_dev_write_word(handle->hdev_, handle->storage_max_offset_+4, *((uint32_t*)&cs.storage_len)) < 0)
00246         return false;
00247 
00248     return true;
00249 }
00250 
00251 static bool create_empty_storage(SOF_DevHandle_t hdev, uint8_t sector_index)
00252 {
00253     SOF_BlockHandle handle_data;
00254 
00255     handle_data.hdev_ = hdev;
00256 
00257     uint32_t info_begin_offset;
00258 
00259     if (!get_empty_info_location(&handle_data, &info_begin_offset)) {
00260         EPRINTF("no info"DCRLF);
00261         SOF_block_close(&handle_data);
00262         return false;
00263     }
00264 
00265     handle_data.storage_max_offset_ = info_begin_offset;
00266     handle_data.storage_begin_offset_ = 0;
00267     handle_data.storage_end_offset_ = handle_data.storage_begin_offset_;
00268     handle_data.cur_pos_ = handle_data.storage_begin_offset_;
00269 
00270     DPRINTF("storage created: begin=%d end=%d free=%d"DCRLF,
00271             handle_data.storage_begin_offset_, handle_data.storage_end_offset_, SOF_block_get_free_size(&handle_data));
00272 
00273     write_storage_info(&handle_data);
00274 
00275     return true;
00276 }
00277 
00278 
00279 bool SOF_block_format(uint8_t sector_index)
00280 {
00281     if (!SOF_dev_is_valid_sector(sector_index)) {
00282         DPRINTF("invalid sector_index=%d"DCRLF, sector_index);
00283         return false;
00284     }
00285 
00286     SOF_DevHandle_t hdev = SOF_dev_open(sector_index);
00287 
00288     if (hdev == SOF_INVALID_HANDLE) {
00289         DPRINTF("SOF_dev_open(%d) failed"DCRLF, sector_index);
00290         return false;
00291     }
00292 
00293     DPRINTF("Flash erase %d"DCRLF, sector_index);
00294     SOF_dev_erase(hdev);
00295     create_empty_storage(hdev, sector_index);
00296     SOF_dev_close(hdev);
00297 
00298     return true;
00299 }
00300 
00301 SOF_BlockHandle_t SOF_block_open_storage(uint8_t sector_index, SOF_Error_t *err)
00302 {
00303     if (!SOF_dev_is_valid_sector(sector_index)) {
00304         DPRINTF("invalid sector_index=%d"DCRLF, sector_index);
00305         return false;
00306     }
00307 
00308     SOF_DevHandle_t hdev = SOF_dev_open(sector_index);
00309 
00310     if (hdev == SOF_INVALID_HANDLE) {
00311         DPRINTF("SOF_dev_open(%d) failed"DCRLF, sector_index);
00312         return false;
00313     }
00314 
00315     SOF_BlockHandle_t handle = new SOF_BlockHandle();
00316 
00317     handle->hdev_ = hdev;
00318 
00319     StorageInfo_t storage_info;
00320 
00321     if ((*err=probe_active_storage_info(handle, &storage_info)) != kSOF_ErrNone) {
00322         delete handle;
00323         return NULL;
00324     }
00325 
00326     uint32_t info_begin_offset;
00327 
00328     if (!get_empty_info_location(handle, &info_begin_offset)) {
00329         *err = kSOF_ErrBadBlock;
00330         delete handle;
00331         return NULL;
00332     }
00333 
00334     // set max offset that storage grows.
00335     handle->storage_max_offset_ = info_begin_offset;
00336 
00337     handle->storage_begin_offset_ = storage_info.begin_offset;
00338     handle->storage_end_offset_ = storage_info.begin_offset + storage_info.len;
00339 
00340     handle->cur_pos_ = handle->storage_begin_offset_;
00341 
00342     DPRINTF("open for read: begin=%d end=%d len=%d free=%d"DCRLF,
00343             handle->storage_begin_offset_, handle->storage_end_offset_, storage_info.len,
00344             SOF_block_get_free_size(handle));
00345     if (compute_storage_checksum(handle) != storage_info.storage_csum) {
00346         EPRINTF("checksum error %#x != %#x"DCRLF, compute_storage_checksum(handle), storage_info.storage_csum);
00347         *err = kSOF_ErrDataCurrupted;
00348         delete handle;
00349         return NULL;
00350     }
00351 
00352     DUMP_BLOCK(handle);
00353     *err = kSOF_ErrNone;
00354 
00355     return handle;
00356 }
00357 
00358 SOF_BlockHandle_t SOF_block_create_storage(uint8_t sector_index, SOF_Error_t *err)
00359 {
00360     if (!SOF_dev_is_valid_sector(sector_index)) {
00361         DPRINTF("invalid sector_index=%d"DCRLF, sector_index);
00362         return false;
00363     }
00364 
00365     SOF_DevHandle_t hdev = SOF_dev_open(sector_index);
00366 
00367     if (hdev == SOF_INVALID_HANDLE) {
00368         DPRINTF("SOF_dev_open(%d) failed"DCRLF, sector_index);
00369         return false;
00370     }
00371 
00372     SOF_BlockHandle_t handle = new SOF_BlockHandle();
00373 
00374     handle->hdev_ = hdev;
00375 
00376     StorageInfo_t storage_info;
00377 
00378     if ((*err=probe_active_storage_info(handle, &storage_info)) != kSOF_ErrNone) {
00379         delete handle;
00380         return NULL;
00381     }
00382 
00383     uint32_t info_begin_offset;
00384 
00385     if (!get_empty_info_location(handle, &info_begin_offset)) {
00386         *err = kSOF_ErrBadBlock;
00387         delete handle;
00388         return NULL;
00389     }
00390 
00391     // set max offset that storage grows.
00392     handle->storage_max_offset_ = info_begin_offset;
00393 
00394     // writing position is just after previous storage
00395     handle->storage_begin_offset_ = storage_info.begin_offset + storage_info.len;
00396     handle->storage_end_offset_ = handle->storage_begin_offset_;
00397 
00398     handle->cur_pos_ = handle->storage_begin_offset_;
00399     handle->write_mode_ = true;
00400     DPRINTF("open for write: begin=%d end=%d free=%d"DCRLF,
00401             handle->storage_begin_offset_, handle->storage_end_offset_, SOF_block_get_free_size(handle));
00402 
00403     DUMP_BLOCK(handle);
00404     *err = kSOF_ErrNone;
00405 
00406     return handle;
00407 }
00408 
00409 bool SOF_block_close(SOF_BlockHandle_t handle)
00410 {
00411     bool r = true;
00412 
00413     DASSERT(handle != NULL);
00414     if (handle->write_mode_)
00415         r = (bool)write_storage_info(handle);
00416     SOF_dev_close(handle->hdev_);
00417     delete handle;
00418 
00419     return r;
00420 }
00421 
00422 uint8_t *SOF_block_base_addr(SOF_BlockHandle_t handle)
00423 {
00424     DASSERT(handle != NULL);
00425     return SOF_dev_get_hw_addr(handle->hdev_) + handle->cur_pos_;
00426 }
00427 
00428 bool SOF_block_putc(SOF_BlockHandle_t handle, uint8_t c)
00429 {
00430     DASSERT(handle != NULL);
00431     DASSERT(handle->is_writable());
00432 
00433     if (SOF_block_get_free_size(handle) == 0) {
00434         DPRINTF("no free space"DCRLF);
00435         DUMP_BLOCK(handle);
00436 
00437         return false;
00438     }
00439 
00440     bool b = SOF_dev_write_byte(handle->hdev_, handle->cur_pos_, c) != -1;
00441     if (b) {
00442         handle->cur_pos_++;
00443         handle->storage_end_offset_++;
00444     }
00445 
00446     return b;
00447 }
00448 
00449 size_t SOF_block_write(SOF_BlockHandle_t handle, const uint8_t *p, size_t p_size)
00450 {
00451     size_t i;
00452 
00453     for (i = 0; i < p_size; ++i)
00454         if (SOF_block_putc(handle, *p++) != true)
00455             return i;
00456 
00457     return i;
00458 }
00459 
00460 bool SOF_block_getc(SOF_BlockHandle_t handle, uint8_t *c)
00461 {
00462     DASSERT(handle != NULL);
00463     DASSERT(handle->is_writable());
00464 
00465     if (handle->cur_pos_ >= handle->storage_end_offset_) {
00466         DPRINTF("end of data\n"DCRLF);
00467         DUMP_BLOCK(handle);
00468 
00469         return false;
00470     }
00471 
00472     *c = SOF_dev_read_byte(handle->hdev_, handle->cur_pos_++);
00473 
00474     return true;
00475 }
00476 
00477 size_t SOF_block_read(SOF_BlockHandle_t handle, uint8_t *p, size_t p_size)
00478 {
00479     size_t i;
00480 
00481     for (i = 0; i < p_size; ++i)
00482         if (!SOF_block_getc(handle, p++))
00483             break;
00484 
00485     return i;
00486 }
00487 
00488 SOF_Error_t SOF_block_get_statics(uint8_t sector_index, SOF_Statics_t *stat)
00489 {
00490     if (!SOF_dev_is_valid_sector(sector_index)) {
00491         DPRINTF("invalid sector_index=%d"DCRLF, sector_index);
00492         return kSOF_ErrParam;
00493     }
00494 
00495     SOF_Error_t err;
00496     SOF_BlockHandle_t hblk = SOF_block_open_storage(sector_index, &err);
00497 
00498     if (hblk == NULL) {
00499         DPRINTF("SOF_block_open_storage(%d) failed"DCRLF, sector_index);
00500         return err;
00501     }
00502 
00503     stat->data_addr = SOF_block_base_addr(hblk);
00504     stat->data_size = SOF_block_storage_size(hblk);
00505     stat->free_size = SOF_block_get_free_size(hblk);
00506 
00507     SOF_block_close(hblk);
00508 
00509     return kSOF_ErrNone;
00510 }
00511 
00512 const SOF_SectorSpec_t *SOF_block_get_info(uint8_t sector_index)
00513 {
00514     if (!SOF_dev_is_valid_sector(sector_index)) {
00515         DPRINTF("invalid sector_index=%d"DCRLF, sector_index);
00516         return NULL;
00517     }
00518 
00519     return SOF_dev_info_by_index(sector_index);
00520 }
00521 
00522