|
| 1 | +/* |
| 2 | + This file is part of the Arduino_AdvancedAnalog library. |
| 3 | + Copyright (c) 2023-2024 Arduino SA. All rights reserved. |
| 4 | +
|
| 5 | + This library is free software; you can redistribute it and/or |
| 6 | + modify it under the terms of the GNU Lesser General Public |
| 7 | + License as published by the Free Software Foundation; either |
| 8 | + version 2.1 of the License, or (at your option) any later version. |
| 9 | +
|
| 10 | + This library is distributed in the hope that it will be useful, |
| 11 | + but WITHOUT ANY WARRANTY; without even the implied warranty of |
| 12 | + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
| 13 | + Lesser General Public License for more details. |
| 14 | +
|
| 15 | + You should have received a copy of the GNU Lesser General Public |
| 16 | + License along with this library; if not, write to the Free Software |
| 17 | + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA |
| 18 | +*/ |
| 19 | + |
| 20 | +#ifndef __DMA_POOL_H__ |
| 21 | +#define__DMA_POOL_H__ |
| 22 | + |
| 23 | +#include<atomic> |
| 24 | + |
| 25 | +namespacearduino { |
| 26 | + |
| 27 | +#if defined(__DCACHE_PRESENT) |
| 28 | +#define__CACHE_LINE_SIZE__ __SCB_DCACHE_LINE_SIZE |
| 29 | +#elif defined(__cpp_lib_hardware_interference_size) |
| 30 | +#define__CACHE_LINE_SIZE__ std::hardware_constructive_interference_size |
| 31 | +#else// No cache. |
| 32 | +#define__CACHE_LINE_SIZE__alignof(int) |
| 33 | +#endif |
| 34 | + |
| 35 | +// Single-producer, single-consumer, lock-free bounded Queue. |
| 36 | +template<classT>classSPSCQueue { |
| 37 | +private: |
| 38 | +size_t capacity; |
| 39 | + std::atomic<size_t> head; |
| 40 | + std::atomic<size_t> tail; |
| 41 | + std::unique_ptr<T[]> buff; |
| 42 | + |
| 43 | +public: |
| 44 | +SPSCQueue(size_t size=0): |
| 45 | +capacity(0), tail(0), head(0), buff(nullptr) { |
| 46 | +if (size) { |
| 47 | + T *mem =new T[size +1]; |
| 48 | +if (mem) { |
| 49 | + buff.reset(mem); |
| 50 | + capacity = size +1; |
| 51 | + } |
| 52 | + } |
| 53 | + } |
| 54 | + |
| 55 | +voidreset() { |
| 56 | + tail = head =0; |
| 57 | + } |
| 58 | + |
| 59 | +size_tempty() { |
| 60 | +return tail == head; |
| 61 | + } |
| 62 | + |
| 63 | +operatorbool()const { |
| 64 | +return buff.get() !=nullptr; |
| 65 | + } |
| 66 | + |
| 67 | +boolpush(T data) { |
| 68 | +size_t curr = head.load(std::memory_order_relaxed); |
| 69 | +size_t next = (curr +1) % capacity; |
| 70 | +if (!buff || (next == tail.load(std::memory_order_acquire))) { |
| 71 | +returnfalse; |
| 72 | + } |
| 73 | + buff[curr] = data; |
| 74 | + head.store(next, std::memory_order_release); |
| 75 | +returntrue; |
| 76 | + } |
| 77 | + |
| 78 | + Tpop(bool peek=false) { |
| 79 | +size_t curr = tail.load(std::memory_order_relaxed); |
| 80 | +if (!buff || (curr == head.load(std::memory_order_acquire))) { |
| 81 | +returnnullptr; |
| 82 | + } |
| 83 | + T data = buff[curr]; |
| 84 | +if (!peek) { |
| 85 | +size_t next = (curr +1) % capacity; |
| 86 | + tail.store(next, std::memory_order_release); |
| 87 | + } |
| 88 | +return data; |
| 89 | + } |
| 90 | +}; |
| 91 | + |
| 92 | +enum { |
| 93 | + DMA_BUFFER_READ = (1 <<0), |
| 94 | + DMA_BUFFER_WRITE = (1 <<1), |
| 95 | + DMA_BUFFER_DISCONT = (1 <<2), |
| 96 | + DMA_BUFFER_INTRLVD = (1 <<3), |
| 97 | +} DMABufferFlags; |
| 98 | + |
| 99 | +// Forward declaration of DMAPool class. |
| 100 | +template<class,size_t>classDMAPool; |
| 101 | + |
| 102 | +template<classT,size_t A=__CACHE_LINE_SIZE__>classDMABuffer { |
| 103 | +private: |
| 104 | + DMAPool<T, A> *pool; |
| 105 | +size_t n_samples; |
| 106 | +size_t n_channels; |
| 107 | + T *ptr; |
| 108 | +uint32_t ts; |
| 109 | +uint32_t flags; |
| 110 | + |
| 111 | +public: |
| 112 | +DMABuffer(DMAPool<T, A> *pool=nullptr,size_t samples=0,size_t channels=0, T *mem=nullptr): |
| 113 | +pool(pool), n_samples(samples), n_channels(channels), ptr(mem), ts(0), flags(0) { |
| 114 | + } |
| 115 | + |
| 116 | + T *data() { |
| 117 | +return ptr; |
| 118 | + } |
| 119 | + |
| 120 | +size_tsize() { |
| 121 | +return n_samples * n_channels; |
| 122 | + } |
| 123 | + |
| 124 | +size_tbytes() { |
| 125 | +return n_samples * n_channels *sizeof(T); |
| 126 | + } |
| 127 | + |
| 128 | +voidflush() { |
| 129 | + #if __DCACHE_PRESENT |
| 130 | +if (ptr) { |
| 131 | +SCB_CleanDCache_by_Addr(data(),bytes()); |
| 132 | + } |
| 133 | + #endif |
| 134 | + } |
| 135 | + |
| 136 | +voidinvalidate() { |
| 137 | + #if __DCACHE_PRESENT |
| 138 | +if (ptr) { |
| 139 | +SCB_InvalidateDCache_by_Addr(data(),bytes()); |
| 140 | + } |
| 141 | + #endif |
| 142 | + } |
| 143 | + |
| 144 | +uint32_ttimestamp() { |
| 145 | +return ts; |
| 146 | + } |
| 147 | + |
| 148 | +voidtimestamp(uint32_t ts) { |
| 149 | +this->ts = ts; |
| 150 | + } |
| 151 | + |
| 152 | +uint32_tchannels() { |
| 153 | +return n_channels; |
| 154 | + } |
| 155 | + |
| 156 | +voidrelease() { |
| 157 | +if (pool && ptr) { |
| 158 | + pool->free(this, flags); |
| 159 | + } |
| 160 | + } |
| 161 | + |
| 162 | +voidset_flags(uint32_t f) { |
| 163 | + flags |= f; |
| 164 | + } |
| 165 | + |
| 166 | +boolget_flags(uint32_t f=0xFFFFFFFFU) { |
| 167 | +return flags & f; |
| 168 | + } |
| 169 | + |
| 170 | +voidclr_flags(uint32_t f=0xFFFFFFFFU) { |
| 171 | + flags &= (~f); |
| 172 | + } |
| 173 | + |
| 174 | + T&operator[](size_t i) { |
| 175 | +assert(ptr && i <size()); |
| 176 | +return ptr[i]; |
| 177 | + } |
| 178 | + |
| 179 | +const T&operator[](size_t i)const { |
| 180 | +assert(ptr && i <size()); |
| 181 | +return ptr[i]; |
| 182 | + } |
| 183 | + |
| 184 | +operatorbool()const { |
| 185 | +return (ptr !=nullptr); |
| 186 | + } |
| 187 | +}; |
| 188 | + |
| 189 | +template<classT,size_t A=__CACHE_LINE_SIZE__>classDMAPool { |
| 190 | +private: |
| 191 | +uint8_t *mem; |
| 192 | +bool managed; |
| 193 | + SPSCQueue<DMABuffer<T>*> wqueue; |
| 194 | + SPSCQueue<DMABuffer<T>*> rqueue; |
| 195 | + |
| 196 | +// Allocates dynamic aligned memory. |
| 197 | +// Note this memory must be free'd with aligned_free. |
| 198 | +staticvoid *aligned_malloc(size_t size) { |
| 199 | +void **ptr, *stashed; |
| 200 | +size_t offset = A -1 +sizeof(void *); |
| 201 | +if ((A %2) || !((stashed = ::malloc(size + offset)))) { |
| 202 | +returnnullptr; |
| 203 | + } |
| 204 | + ptr = (void **) (((uintptr_t) stashed + offset) & ~(A -1)); |
| 205 | + ptr[-1] = stashed; |
| 206 | +return ptr; |
| 207 | + } |
| 208 | + |
| 209 | +// Frees dynamic aligned memory allocated with aligned_malloc. |
| 210 | +staticvoidaligned_free(void *ptr) { |
| 211 | +if (ptr !=nullptr) { |
| 212 | +::free(((void **) ptr)[-1]); |
| 213 | + } |
| 214 | + } |
| 215 | + |
| 216 | +public: |
| 217 | +DMAPool(size_t n_samples,size_t n_channels,size_t n_buffers,void *mem_in=nullptr): |
| 218 | +mem((uint8_t *) mem_in), managed(mem_in==nullptr), wqueue(n_buffers), rqueue(n_buffers) { |
| 219 | +// Round up to the next multiple of the alignment. |
| 220 | +size_t bufsize = (((n_samples * n_channels *sizeof(T)) + (A-1)) & ~(A-1)); |
| 221 | +if (bufsize && rqueue && wqueue) { |
| 222 | +if (mem ==nullptr) { |
| 223 | +// Allocate an aligned memory block for the DMA buffers' memory. |
| 224 | + mem = (uint8_t *)aligned_malloc(n_buffers * bufsize); |
| 225 | +if (!mem) { |
| 226 | +// Failed to allocate memory. |
| 227 | +return; |
| 228 | + } |
| 229 | + } |
| 230 | +// Allocate the DMA buffers, initialize them using aligned |
| 231 | +// pointers from the pool, and add them to the write queue. |
| 232 | +for (size_t i=0; i<n_buffers; i++) { |
| 233 | + DMABuffer<T> *buf =new DMABuffer<T>( |
| 234 | +this, n_samples, n_channels, (T *) &mem[i * bufsize] |
| 235 | + ); |
| 236 | +if (buf ==nullptr) { |
| 237 | +break; |
| 238 | + } |
| 239 | + wqueue.push(buf); |
| 240 | + } |
| 241 | + } |
| 242 | + } |
| 243 | + |
| 244 | +~DMAPool() { |
| 245 | +while (readable()) { |
| 246 | +deletealloc(DMA_BUFFER_READ); |
| 247 | + } |
| 248 | + |
| 249 | +while (writable()) { |
| 250 | +deletealloc(DMA_BUFFER_WRITE); |
| 251 | + } |
| 252 | + |
| 253 | +if (mem && managed) { |
| 254 | +aligned_free(mem); |
| 255 | + } |
| 256 | + } |
| 257 | + |
| 258 | +boolwritable() { |
| 259 | +return !(wqueue.empty()); |
| 260 | + } |
| 261 | + |
| 262 | +boolreadable() { |
| 263 | +return !(rqueue.empty()); |
| 264 | + } |
| 265 | + |
| 266 | +voidflush() { |
| 267 | +while (readable()) { |
| 268 | + DMABuffer<T> *buf =alloc(DMA_BUFFER_READ); |
| 269 | +if (buf) { |
| 270 | + buf->release(); |
| 271 | + } |
| 272 | + } |
| 273 | + } |
| 274 | + |
| 275 | + DMABuffer<T> *alloc(uint32_t flags) { |
| 276 | + DMABuffer<T> *buf =nullptr; |
| 277 | +if (flags & DMA_BUFFER_READ) { |
| 278 | +// Get a DMA buffer from the read/ready queue. |
| 279 | + buf = rqueue.pop(); |
| 280 | + }else { |
| 281 | +// Get a DMA buffer from the write/free queue. |
| 282 | + buf = wqueue.pop(); |
| 283 | + } |
| 284 | +if (buf) { |
| 285 | + buf->clr_flags(DMA_BUFFER_READ | DMA_BUFFER_WRITE); |
| 286 | + buf->set_flags(flags); |
| 287 | + } |
| 288 | +return buf; |
| 289 | + } |
| 290 | + |
| 291 | +voidfree(DMABuffer<T> *buf,uint32_t flags=0) { |
| 292 | +if (buf ==nullptr) { |
| 293 | +return; |
| 294 | + } |
| 295 | +if (flags ==0) { |
| 296 | + flags = buf->get_flags(); |
| 297 | + } |
| 298 | +if (flags & DMA_BUFFER_READ) { |
| 299 | +// Return the DMA buffer to the write/free queue. |
| 300 | + buf->clr_flags(); |
| 301 | + wqueue.push(buf); |
| 302 | + }else { |
| 303 | +// Return the DMA buffer to the read/ready queue. |
| 304 | + rqueue.push(buf); |
| 305 | + } |
| 306 | + } |
| 307 | + |
| 308 | +}; |
| 309 | + |
| 310 | +}// namespace arduino |
| 311 | + |
| 312 | +using arduino::DMAPool; |
| 313 | +using arduino::DMABuffer; |
| 314 | +using arduino::SPSCQueue; |
| 315 | +#endif//__DMA_POOL_H__ |