diff --git a/library/ringbuffer/ringbuffer.c b/library/ringbuffer/ringbuffer.c index 20e0020..92e385a 100644 --- a/library/ringbuffer/ringbuffer.c +++ b/library/ringbuffer/ringbuffer.c @@ -120,35 +120,46 @@ int rd_RingbufferPut(rd_ringbuf_t *rb, const char *ptr, int length) { if (!rb || !ptr || length <= 0) return RD_INVALUE; - unsigned short wi = ATOMIC_LOAD(&rb->m_sWriteIndex, unsigned short, ATOMIC_ORDER_RELAXED); - unsigned short ri = ATOMIC_LOAD(&rb->m_sReadIndex, unsigned short, ATOMIC_ORDER_RELAXED); - unsigned char wb = ATOMIC_LOAD(&rb->m_bWriteMirror, unsigned char, ATOMIC_ORDER_RELAXED); - unsigned char rb_m = ATOMIC_LOAD(&rb->m_bReadMirror, unsigned char, ATOMIC_ORDER_RELAXED); + unsigned short wi, ri; + unsigned char wb, rb_m; + int space, tail; + unsigned int primask; + + __asm__ volatile ("mrs %0, primask" : "=r"(primask)); + __asm__ volatile ("cpsid i" ::: "memory"); + + wi = ATOMIC_LOAD(&rb->m_sWriteIndex, unsigned short, ATOMIC_ORDER_RELAXED); + ri = ATOMIC_LOAD(&rb->m_sReadIndex, unsigned short, ATOMIC_ORDER_RELAXED); + wb = ATOMIC_LOAD(&rb->m_bWriteMirror, unsigned char, ATOMIC_ORDER_RELAXED); + rb_m = ATOMIC_LOAD(&rb->m_bReadMirror, unsigned char, ATOMIC_ORDER_RELAXED); - int space; if (wb == rb_m) { space = rb->m_iBufsize - (int)(unsigned short)(wi - ri); } else { space = (int)(unsigned short)(ri - wi); } - if (space <= 0) return 0; + if (space <= 0) { + __asm__ volatile ("msr primask, %0" :: "r"(primask) : "memory"); + return 0; + } if (space < length) length = space; - /* 写入不跨越尾部 */ - int tail = rb->m_iBufsize - wi; + tail = rb->m_iBufsize - wi; if (tail > length) { RD_MEMCPY(&rb->m_pcBufPtr[wi], ptr, length); wi += length; } else { RD_MEMCPY(&rb->m_pcBufPtr[wi], ptr, tail); RD_MEMCPY(&rb->m_pcBufPtr[0], ptr + tail, length - tail); - wb ^= 1; /* 翻转镜像位 */ + wb ^= 1; wi = length - tail; } ATOMIC_STORE(&rb->m_sWriteIndex, wi, unsigned short, ATOMIC_ORDER_RELEASE); ATOMIC_STORE(&rb->m_bWriteMirror, wb, unsigned char, ATOMIC_ORDER_RELEASE); + + __asm__ volatile ("msr primask, %0" :: "r"(primask) : "memory"); return length; } @@ -165,6 +176,10 @@ int rd_RingbufferPutForce(rd_ringbuf_t *rb, const char *ptr, int length) { if (!rb || !ptr || length <= 0) return RD_INVALUE; + unsigned int primask; + __asm__ volatile ("mrs %0, primask" : "=r"(primask)); + __asm__ volatile ("cpsid i" ::: "memory"); + int space = rd_RingbufferSpaceLen(rb); if (length > rb->m_iBufsize) { ptr += length - rb->m_iBufsize; @@ -174,18 +189,18 @@ int rd_RingbufferPutForce(rd_ringbuf_t *rb, const char *ptr, int length) unsigned short wi = ATOMIC_LOAD(&rb->m_sWriteIndex, unsigned short, ATOMIC_ORDER_RELAXED); unsigned char wb = ATOMIC_LOAD(&rb->m_bWriteMirror, unsigned char, ATOMIC_ORDER_RELAXED); - if (rb->m_iBufsize - wi > length) { + int tail = rb->m_iBufsize - wi; + if (tail > length) { RD_MEMCPY(&rb->m_pcBufPtr[wi], ptr, length); wi += length; if (length > space) { ATOMIC_STORE(&rb->m_sReadIndex, wi, unsigned short, ATOMIC_ORDER_RELEASE); } } else { - int first = rb->m_iBufsize - wi; - RD_MEMCPY(&rb->m_pcBufPtr[wi], ptr, first); - RD_MEMCPY(&rb->m_pcBufPtr[0], ptr + first, length - first); + RD_MEMCPY(&rb->m_pcBufPtr[wi], ptr, tail); + RD_MEMCPY(&rb->m_pcBufPtr[0], ptr + tail, length - tail); wb ^= 1; - wi = length - first; + wi = length - tail; if (length > space) { unsigned short ri = ATOMIC_LOAD(&rb->m_sReadIndex, unsigned short, ATOMIC_ORDER_RELAXED); if (wi <= ri) wb ^= 1; @@ -195,6 +210,8 @@ int rd_RingbufferPutForce(rd_ringbuf_t *rb, const char *ptr, int length) ATOMIC_STORE(&rb->m_sWriteIndex, wi, unsigned short, ATOMIC_ORDER_RELEASE); ATOMIC_STORE(&rb->m_bWriteMirror, wb, unsigned char, ATOMIC_ORDER_RELEASE); + + __asm__ volatile ("msr primask, %0" :: "r"(primask) : "memory"); return length; } @@ -296,7 +313,15 @@ int rd_RingbufferPeekLinear(rd_ringbuf_t *rb, char *out, int length) int rd_RingbufferPutchar(rd_ringbuf_t *rb, const char ch) { if (!rb) return RD_INVALUE; - if (!rd_RingbufferSpaceLen(rb)) return 0; + + unsigned int primask; + __asm__ volatile ("mrs %0, primask" : "=r"(primask)); + __asm__ volatile ("cpsid i" ::: "memory"); + + if (rd_RingbufferSpaceLen(rb) == 0) { + __asm__ volatile ("msr primask, %0" :: "r"(primask) : "memory"); + return 0; + } unsigned short wi = ATOMIC_LOAD(&rb->m_sWriteIndex, unsigned short, ATOMIC_ORDER_RELAXED); unsigned char wb = ATOMIC_LOAD(&rb->m_bWriteMirror, unsigned char, ATOMIC_ORDER_RELAXED); @@ -311,6 +336,8 @@ int rd_RingbufferPutchar(rd_ringbuf_t *rb, const char ch) ATOMIC_STORE(&rb->m_sWriteIndex, wi, unsigned short, ATOMIC_ORDER_RELEASE); ATOMIC_STORE(&rb->m_bWriteMirror, wb, unsigned char, ATOMIC_ORDER_RELEASE); + + __asm__ volatile ("msr primask, %0" :: "r"(primask) : "memory"); return 1; } @@ -326,6 +353,10 @@ int rd_RingbufferPutcharForce(rd_ringbuf_t *rb, const char ch) { if (!rb) return RD_INVALUE; + unsigned int primask; + __asm__ volatile ("mrs %0, primask" : "=r"(primask)); + __asm__ volatile ("cpsid i" ::: "memory"); + enum ERingBufState old_state = rd_RingbufferStatus(rb); unsigned short wi = ATOMIC_LOAD(&rb->m_sWriteIndex, unsigned short, ATOMIC_ORDER_RELAXED); unsigned char wb = ATOMIC_LOAD(&rb->m_bWriteMirror, unsigned char, ATOMIC_ORDER_RELAXED); @@ -349,6 +380,8 @@ int rd_RingbufferPutcharForce(rd_ringbuf_t *rb, const char ch) ATOMIC_STORE(&rb->m_sWriteIndex, wi, unsigned short, ATOMIC_ORDER_RELEASE); ATOMIC_STORE(&rb->m_bWriteMirror, wb, unsigned char, ATOMIC_ORDER_RELEASE); + + __asm__ volatile ("msr primask, %0" :: "r"(primask) : "memory"); return 1; }