avoid modulo

Author: paul-szczepanek-arm

platform/include/platform/CircularBuffer.h

@@ -114,6 +114,8 @@ class CircularBuffer {
      */
     void push(const T *src, CounterType len)
     {
+        MBED_ASSERT(len > 0);
+
         core_util_critical_section_enter();
 
         /* if we try to write more bytes than the buffer can hold we only bother writing the last bytes */
@@ -129,23 +131,21 @@ class CircularBuffer {
             CounterType written = len;
 
             /* on first pass we write as much as we can to the right of head */
-            if ((_head + len) > BufferSize) {
+            if ((_head + written) > BufferSize) {
                 written = BufferSize - _head;
             }
 
             std::copy(src, src + written, _buffer + _head);
+            _head = incrementCounter(_head, written);
 
             CounterType left_to_write = len - written;
 
             /* we might need to continue to write from the start of the buffer */
             if (left_to_write) {
                 std::copy(src + written, src + written + left_to_write, _buffer);
-                written += left_to_write;
+                _head = left_to_write;
             }
 
-            _head += written;
-            _head %= BufferSize;
-
             if (adjust_tail) {
                 _tail = _head;
                 _full = true;
@@ -198,6 +198,8 @@ class CircularBuffer {
      */
     CounterType pop(T *dest, CounterType len)
     {
+        MBED_ASSERT(len > 0);
+
         if (len == 0) {
             return 0;
         }
@@ -219,18 +221,18 @@ class CircularBuffer {
             }
 
             std::copy(_buffer + _tail, _buffer + _tail + data_popped, dest);
+            _tail = incrementCounter(_tail, data_popped);
 
             /* if we looped over the end we may need to pop again */
             CounterType left_to_pop = len - data_popped;
 
             if (left_to_pop) {
                 std::copy(_buffer, _buffer + left_to_pop, dest + data_popped);
+                _tail = left_to_pop;
 
                 data_popped += left_to_pop;
             }
 
-            _tail += data_popped;
-            _tail %= BufferSize;
             _full = false;
         }
 
@@ -335,9 +337,23 @@ class CircularBuffer {
         return elements;
     }
 
-    CounterType incrementCounter(CounterType val)
+    /** Used to increment _tail or _head by a given value.
+     *
+     * @param val The value of the counter to be incremented.
+     * @param increment The amount to be added, the value after this incremented must not exceed BufferSize.
+     * @return The new value of the counter.
+     */
+    CounterType incrementCounter(CounterType val, CounterType increment = 1)
     {
-        return (++val) % BufferSize;
+        val += increment;
+
+        MBED_ASSERT(val <= BufferSize);
+
+        if (val == BufferSize) {
+            val = 0;
+        }
+
+        return val;
     }
 
 private: