USBSerialBuffered.cpp

00001 /*
00002  * $Id: bulkserial.h,v 1.3 2018/02/23 15:04:29 grimrath Exp $
00003  * This is an unpublished work copyright (c) 2017 HELIOS Software GmbH
00004  * 30827 Garbsen, Germany
00005  */
00006 
00007 #include <algorithm>
00008 #include <mbed.h>
00009 #include "PinMap.h"
00010 
00011 #ifdef FEATURE_USBSERIAL
00012 #include "USBSerial.h"
00013 #include "USBSerialBuffered.h"
00014 
00015 #ifndef ASSERT
00016 #define ASSERT          MBED_ASSERT
00017 #endif
00018 #ifndef POISONMEM
00019 static inline void POISONMEM(void *ptr, size_t sz) {
00020         memset(ptr, 0x55, sz);
00021 }
00022 #endif
00023 #if defined(__ATOMIC_RELAXED)
00024 
00025 #define help_atomic_load_relaxed(ptr) __atomic_load_n((ptr), __ATOMIC_RELAXED)
00026 
00027 #define help_atomic_store_relaxed(ptr, val) __atomic_store_n((ptr), (val), __ATOMIC_RELAXED)
00028 
00029 #define help_atomic_readclr_relaxed(ptr) __atomic_exchange_n((ptr), 0, __ATOMIC_RELAXED)
00030 
00031 #define help_atomic_or_relaxed(ptr, val) __atomic_fetch_or((ptr), (val), __ATOMIC_RELAXED)
00032 
00033 #ifdef __cplusplus
00034 template<typename T> inline bool help_atomic_compare_and_swap(T *ptr, T checkval, T newval) {
00035     return __atomic_compare_exchange_n(ptr, &checkval, newval, false, __ATOMIC_RELAXED, __ATOMIC_RELAXED);
00036 }
00037 #else
00038 #define help_atomic_compare_and_swap(ptr, checkval, newval) __sync_bool_compare_and_swap((ptr), (checkval), (newval))
00039 #endif
00040 
00041 #define sync_memory(mem) do { \
00042     asm volatile("" : "=m" (mem)); \
00043     __atomic_thread_fence(__ATOMIC_SEQ_CST); \
00044 } while (0)
00045 
00046 #define irq_barrier() __atomic_signal_fence(__ATOMIC_SEQ_CST)
00047 
00048 #define sync_memory_all() do { \
00049     asm volatile("" : : : "memory"); \
00050     __atomic_thread_fence(__ATOMIC_SEQ_CST); \
00051 } while (0)
00052 
00053 #else // defined(__ATOMIC_RELAXED)
00054 
00055 #define help_atomic_load_relaxed(ptr) (*(ptr))
00056 
00057 #define help_atomic_store_relaxed(ptr, val) ((void)(*(ptr) = (val)))
00058 
00059 #define help_atomic_readclr_relaxed(ptr) __sync_fetch_and_and((ptr), 0)
00060 
00061 #define help_atomic_or_relaxed(ptr, val) __sync_fetch_and_or((ptr), (val))
00062 
00063 #define help_atomic_compare_and_swap(ptr, checkval, newval) __sync_bool_compare_and_swap((ptr), (checkval), (newval))
00064 
00065 #define sync_memory(mem) __sync_synchronize()
00066 
00067 #define sync_memory_all() __sync_synchronize()
00068 
00069 #define irq_barrier() __sync_synchronize()
00070 
00071 #endif
00072 
00073 
00074 USBSerialBuffered::USBSerialBuffered(int MaxBuffSize, uint16_t vendor_id, uint16_t product_id, uint16_t product_release, bool connect_blocking)
00075 : USBSerial(vendor_id, product_id, product_release, connect_blocking)
00076 , mFullBuffSize(MaxBuffSize)
00077 {
00078     ASSERT(mFullBuffSize > CorkBuffSize && "FullBuff must be larger than CorkBuff");
00079     m_buff = new char[mFullBuffSize];
00080     m_irq_buffused = 0;
00081     POISONMEM(m_buff, mFullBuffSize);
00082 }
00083 
00084 USBSerialBuffered::~USBSerialBuffered() {
00085     delete[] m_buff;
00086 }
00087 
00088 //-----------------------------------------------------------------------------
00089 
00090 int USBSerialBuffered::irqbuff_acquire() {
00091     core_util_critical_section_enter();
00092     return help_atomic_load_relaxed(&m_irq_buffused);
00093 }
00094 
00095 void USBSerialBuffered::irqbuff_release(int buffused) {
00096     help_atomic_store_relaxed(&m_irq_buffused, buffused);
00097     irq_barrier();
00098     core_util_critical_section_exit();
00099 }
00100 
00101 //-----------------------------------------------------------------------------
00102 
00103 int USBSerialBuffered::printf_irqsafe(const char *fmt, ...) {
00104     std::va_list va;
00105     va_start(va, fmt);
00106     int nchars = vprintf_irqsafe(fmt, va);
00107     va_end(va);
00108     return nchars;
00109 }
00110 
00111 int USBSerialBuffered::vprintf_irqsafe(const char *fmt, std::va_list va) {
00112     if (RunningInInterrupt()) {
00113         int buffused = irqbuff_acquire();
00114         int bspc = mFullBuffSize - buffused;
00115         ASSERT(bspc >= 0);
00116         int nchars = vsnprintf(m_buff + buffused, bspc, fmt, va);
00117         if (nchars >= bspc) {
00118             memcpy(m_buff + mFullBuffSize - 4, "...\n", 4);
00119             buffused = mFullBuffSize;
00120         } else {
00121             buffused += nchars;
00122         }
00123         irqbuff_release(buffused);
00124         return nchars;
00125     } else {
00126         return USBSerial::vprintf(fmt, va);
00127     }
00128 }
00129 
00130 //-----------------------------------------------------------------------------
00131 
00132 void USBSerialBuffered::flush() {
00133     int flushedbytes = 0;
00134     int buffused = 0;
00135     while (1) {
00136         bool wasequal = help_atomic_compare_and_swap(&m_irq_buffused, buffused, 0);
00137         if (wasequal)
00138             break;
00139         //
00140         // This only works because @ref print_irq always _increases_ @c m_irq_buffused (but never(!) decreases this variable)
00141         //
00142         buffused = help_atomic_load_relaxed(&m_irq_buffused);   // __sync_* implementation requires this refetch
00143         while (buffused != flushedbytes) {
00144             int towrite = std::min(buffused - flushedbytes, static_cast<int>(CorkBuffSize));
00145             if (connected()) {
00146                 writeBlock(reinterpret_cast<uint8_t *>(m_buff + flushedbytes), towrite);
00147             }
00148             flushedbytes += towrite;
00149         }
00150     }
00151 }
00152 
00153 void USBSerialBuffered::putc_normal(int c) {
00154     while (1) {
00155         int buffused = help_atomic_load_relaxed(&m_irq_buffused);
00156         if (buffused >= CorkBuffSize) {
00157             flush();
00158         } else {
00159 //             static bool TESTonce;
00160 //             if (! TESTonce) {
00161 //                 printf_irqsafe("ppp");
00162 //                 TESTonce = true;
00163 //             }
00164             ASSERT(buffused + 1 <= CorkBuffSize);
00165             bool wasequal = help_atomic_compare_and_swap(&m_irq_buffused, buffused, buffused + 1);
00166             if (wasequal) {
00167                 m_buff[buffused] = c;   // alloc successful
00168                 return;
00169             } else {
00170                 // An irq extended m_irq_buffused, start over
00171             }
00172         }
00173     }
00174 }
00175 
00176 int USBSerialBuffered::_putc(int c) {
00177     putc_normal(c);
00178     if (c == '\n') {
00179         flush();
00180     }
00181     return connected() ? 1 : 0;
00182 }
00183 
00184 #endif