uc_transfer_buffer.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan/transport/transfer_buffer.hpp>
00006 #include <cassert>
00007 #include <cstdlib>
00008 
00009 namespace uavcan
00010 {
00011 /*
00012  * StaticTransferBufferImpl
00013  */
00014 int StaticTransferBufferImpl::read(unsigned offset, uint8_t* data, unsigned len) const
00015 {
00016     if (!data)
00017     {
00018         UAVCAN_ASSERT(0);
00019         return -ErrInvalidParam;
00020     }
00021     if (offset >= max_write_pos_)
00022     {
00023         return 0;
00024     }
00025     if ((offset + len) > max_write_pos_)
00026     {
00027         len = max_write_pos_ - offset;
00028     }
00029     UAVCAN_ASSERT((offset + len) <= max_write_pos_);
00030     (void)copy(data_ + offset, data_ + offset + len, data);
00031     return int(len);
00032 }
00033 
00034 int StaticTransferBufferImpl::write(unsigned offset, const uint8_t* data, unsigned len)
00035 {
00036     if (!data)
00037     {
00038         UAVCAN_ASSERT(0);
00039         return -ErrInvalidParam;
00040     }
00041     if (offset >= size_)
00042     {
00043         return 0;
00044     }
00045     if ((offset + len) > size_)
00046     {
00047         len = size_ - offset;
00048     }
00049     UAVCAN_ASSERT((offset + len) <= size_);
00050     (void)copy(data, data + len, data_ + offset);
00051     max_write_pos_ = max(uint16_t(offset + len), uint16_t(max_write_pos_));
00052     return int(len);
00053 }
00054 
00055 void StaticTransferBufferImpl::reset()
00056 {
00057     max_write_pos_ = 0;
00058 #if UAVCAN_DEBUG
00059     fill(data_, data_ + size_, uint8_t(0));
00060 #endif
00061 }
00062 
00063 /*
00064  * TransferBufferManagerKey
00065  */
00066 #if UAVCAN_TOSTRING
00067 std::string TransferBufferManagerKey::toString() const
00068 {
00069     char buf[24];
00070     (void)snprintf(buf, sizeof(buf), "nid=%i tt=%i", int(node_id_.get()), int(transfer_type_));
00071     return std::string(buf);
00072 }
00073 #endif
00074 
00075 /*
00076  * DynamicTransferBuffer::Block
00077  */
00078 TransferBufferManagerEntry::Block*
00079 TransferBufferManagerEntry::Block::instantiate(IPoolAllocator& allocator)
00080 {
00081     void* const praw = allocator.allocate(sizeof(Block));
00082     if (praw == UAVCAN_NULLPTR)
00083     {
00084         return UAVCAN_NULLPTR;
00085     }
00086     return new (praw) Block;
00087 }
00088 
00089 void TransferBufferManagerEntry::Block::destroy(Block*& obj, IPoolAllocator& allocator)
00090 {
00091     if (obj != UAVCAN_NULLPTR)
00092     {
00093         obj->~Block();
00094         allocator.deallocate(obj);
00095         obj = UAVCAN_NULLPTR;
00096     }
00097 }
00098 
00099 void TransferBufferManagerEntry::Block::read(uint8_t*& outptr, unsigned target_offset,
00100                                              unsigned& total_offset, unsigned& left_to_read)
00101 {
00102     UAVCAN_ASSERT(outptr);
00103     for (unsigned i = 0; (i < Block::Size) && (left_to_read > 0); i++, total_offset++)
00104     {
00105         if (total_offset >= target_offset)
00106         {
00107             *outptr++ = data[i];
00108             left_to_read--;
00109         }
00110     }
00111 }
00112 
00113 void TransferBufferManagerEntry::Block::write(const uint8_t*& inptr, unsigned target_offset,
00114                                                      unsigned& total_offset, unsigned& left_to_write)
00115 {
00116     UAVCAN_ASSERT(inptr);
00117     for (unsigned i = 0; (i < Block::Size) && (left_to_write > 0); i++, total_offset++)
00118     {
00119         if (total_offset >= target_offset)
00120         {
00121             data[i] = *inptr++;
00122             left_to_write--;
00123         }
00124     }
00125 }
00126 
00127 /*
00128  * DynamicTransferBuffer
00129  */
00130 TransferBufferManagerEntry* TransferBufferManagerEntry::instantiate(IPoolAllocator& allocator,
00131                                                                                   uint16_t max_size)
00132 {
00133     void* const praw = allocator.allocate(sizeof(TransferBufferManagerEntry));
00134     if (praw == UAVCAN_NULLPTR)
00135     {
00136         return UAVCAN_NULLPTR;
00137     }
00138     return new (praw) TransferBufferManagerEntry(allocator, max_size);
00139 }
00140 
00141 void TransferBufferManagerEntry::destroy(TransferBufferManagerEntry*& obj, IPoolAllocator& allocator)
00142 {
00143     if (obj != UAVCAN_NULLPTR)
00144     {
00145         obj->~TransferBufferManagerEntry();
00146         allocator.deallocate(obj);
00147         obj = UAVCAN_NULLPTR;
00148     }
00149 }
00150 
00151 int TransferBufferManagerEntry::read(unsigned offset, uint8_t* data, unsigned len) const
00152 {
00153     if (!data)
00154     {
00155         UAVCAN_ASSERT(0);
00156         return -ErrInvalidParam;
00157     }
00158     if (offset >= max_write_pos_)
00159     {
00160         return 0;
00161     }
00162     if ((offset + len) > max_write_pos_)
00163     {
00164         len = max_write_pos_ - offset;
00165     }
00166     UAVCAN_ASSERT((offset + len) <= max_write_pos_);
00167 
00168     // This shall be optimized.
00169     unsigned total_offset = 0;
00170     unsigned left_to_read = len;
00171     uint8_t* outptr = data;
00172     Block* p = blocks_.get();
00173     while (p)
00174     {
00175         p->read(outptr, offset, total_offset, left_to_read);
00176         if (left_to_read == 0)
00177         {
00178             break;
00179         }
00180         p = p->getNextListNode();
00181     }
00182 
00183     UAVCAN_ASSERT(left_to_read == 0);
00184     return int(len);
00185 }
00186 
00187 int TransferBufferManagerEntry::write(unsigned offset, const uint8_t* data, unsigned len)
00188 {
00189     if (!data)
00190     {
00191         UAVCAN_ASSERT(0);
00192         return -ErrInvalidParam;
00193     }
00194 
00195     if (offset >= max_size_)
00196     {
00197         return 0;
00198     }
00199     if ((offset + len) > max_size_)
00200     {
00201         len = max_size_ - offset;
00202     }
00203     UAVCAN_ASSERT((offset + len) <= max_size_);
00204 
00205     unsigned total_offset = 0;
00206     unsigned left_to_write = len;
00207     const uint8_t* inptr = data;
00208     Block* p = blocks_.get();
00209     Block* last_written_block = UAVCAN_NULLPTR;
00210 
00211     // First we need to write the part that is already allocated
00212     while (p)
00213     {
00214         last_written_block = p;
00215         p->write(inptr, offset, total_offset, left_to_write);
00216         if (left_to_write == 0)
00217         {
00218             break;
00219         }
00220         p = p->getNextListNode();
00221     }
00222 
00223     // Then we need to append new chunks until all data is written
00224     while (left_to_write > 0)
00225     {
00226         // cppcheck-suppress nullPointer
00227         UAVCAN_ASSERT(p == UAVCAN_NULLPTR);
00228 
00229         // Allocating the chunk
00230         Block* new_block = Block::instantiate(allocator_);
00231         if (new_block == UAVCAN_NULLPTR)
00232         {
00233             break;                        // We're in deep shit.
00234         }
00235         // Appending the chain with the new block
00236         if (last_written_block != UAVCAN_NULLPTR)
00237         {
00238             UAVCAN_ASSERT(last_written_block->getNextListNode() == UAVCAN_NULLPTR);  // Because it is last in the chain
00239             last_written_block->setNextListNode(new_block);
00240             new_block->setNextListNode(UAVCAN_NULLPTR);
00241         }
00242         else
00243         {
00244             blocks_.insert(new_block);
00245         }
00246         last_written_block = new_block;
00247 
00248         // Writing the data
00249         new_block->write(inptr, offset, total_offset, left_to_write);
00250     }
00251 
00252     UAVCAN_ASSERT(len >= left_to_write);
00253     const unsigned actually_written = len - left_to_write;
00254     max_write_pos_ = max(uint16_t(offset + actually_written), uint16_t(max_write_pos_));
00255     return int(actually_written);
00256 }
00257 
00258 void TransferBufferManagerEntry::reset(const TransferBufferManagerKey& key)
00259 {
00260     key_ = key;
00261     max_write_pos_ = 0;
00262     Block* p = blocks_.get();
00263     while (p)
00264     {
00265         Block* const next = p->getNextListNode();
00266         blocks_.remove(p);
00267         Block::destroy(p, allocator_);
00268         p = next;
00269     }
00270 }
00271 
00272 /*
00273  * TransferBufferManager
00274  */
00275 TransferBufferManagerEntry* TransferBufferManager::findFirst(const TransferBufferManagerKey& key)
00276 {
00277     TransferBufferManagerEntry* dyn = buffers_.get();
00278     while (dyn)
00279     {
00280         UAVCAN_ASSERT(!dyn->isEmpty());
00281         if (dyn->getKey() == key)
00282         {
00283             return dyn;
00284         }
00285         dyn = dyn->getNextListNode();
00286     }
00287     return UAVCAN_NULLPTR;
00288 }
00289 
00290 TransferBufferManager::~TransferBufferManager()
00291 {
00292     TransferBufferManagerEntry* dyn = buffers_.get();
00293     while (dyn)
00294     {
00295         TransferBufferManagerEntry* const next = dyn->getNextListNode();
00296         buffers_.remove(dyn);
00297         TransferBufferManagerEntry::destroy(dyn, allocator_);
00298         dyn = next;
00299     }
00300 }
00301 
00302 ITransferBuffer* TransferBufferManager::access(const TransferBufferManagerKey& key)
00303 {
00304     if (key.isEmpty())
00305     {
00306         UAVCAN_ASSERT(0);
00307         return UAVCAN_NULLPTR;
00308     }
00309     return findFirst(key);
00310 }
00311 
00312 ITransferBuffer* TransferBufferManager::create(const TransferBufferManagerKey& key)
00313 {
00314     if (key.isEmpty())
00315     {
00316         UAVCAN_ASSERT(0);
00317         return UAVCAN_NULLPTR;
00318     }
00319     remove(key);
00320 
00321     TransferBufferManagerEntry* tbme = TransferBufferManagerEntry::instantiate(allocator_, max_buf_size_);
00322     if (tbme == UAVCAN_NULLPTR)
00323     {
00324         return UAVCAN_NULLPTR;     // Epic fail.
00325     }
00326 
00327     buffers_.insert(tbme);
00328 
00329     UAVCAN_TRACE("TransferBufferManager", "Buffer created [num=%u], %s", getNumBuffers(), key.toString().c_str());
00330 
00331     if (tbme != UAVCAN_NULLPTR)
00332     {
00333         UAVCAN_ASSERT(tbme->isEmpty());
00334         tbme->reset(key);
00335     }
00336     return tbme;
00337 }
00338 
00339 void TransferBufferManager::remove(const TransferBufferManagerKey& key)
00340 {
00341     UAVCAN_ASSERT(!key.isEmpty());
00342 
00343     TransferBufferManagerEntry* dyn = findFirst(key);
00344     if (dyn != UAVCAN_NULLPTR)
00345     {
00346         UAVCAN_TRACE("TransferBufferManager", "Buffer deleted, %s", key.toString().c_str());
00347         buffers_.remove(dyn);
00348         TransferBufferManagerEntry::destroy(dyn, allocator_);
00349     }
00350 }
00351 
00352 bool TransferBufferManager::isEmpty() const
00353 {
00354     return getNumBuffers() == 0;
00355 }
00356 
00357 unsigned TransferBufferManager::getNumBuffers() const
00358 {
00359     return buffers_.getLength();
00360 }
00361 
00362 }