[tcp] implement otTcpReceiveContiguify (#7634)

src/core/net/tcp6.cpp

@@ -234,7 +234,11 @@ Error Tcp::Endpoint::ReceiveByReference(const otLinkedBuffer *&aBuffer)
 
 Error Tcp::Endpoint::ReceiveContiguify(void)
 {
-    return kErrorNotImplemented;
+    struct tcpcb &tp = GetTcb();
+
+    cbuf_contiguify(&tp.recvbuf, tp.reassbmp);
+
+    return kErrorNone;
 }
 
 Error Tcp::Endpoint::CommitReceive(size_t aNumBytes, uint32_t aFlags)

third_party/tcplp/lib/bitmap.c

@@ -135,6 +135,67 @@ size_t bmp_countset(uint8_t* buf, size_t buflen, size_t start, size_t limit) {
     return numset;
 }
 
+static inline uint8_t bmp_read_bit(uint8_t* buf, size_t i) {
+    size_t byte_index = i >> 3;
+    size_t bit_index = i & 0x7; // Amount to left shift to get bit in MSB
+    return ((uint8_t) (buf[byte_index] << bit_index)) >> 7;
+}
+
+static inline void bmp_write_bit(uint8_t* buf, size_t i, uint8_t bit) {
+    size_t byte_index = i >> 3;
+    size_t bit_index = i & 0x7; // Amount to left shift to get bit in MSB
+    size_t bit_shift = 7 - bit_index; // Amount to right shift to get bit in LSB
+    buf[byte_index] = (buf[byte_index] & ~(1 << bit_shift)) | (bit << bit_shift);
+}
+
+static inline uint8_t bmp_read_byte(uint8_t* buf, size_t i) {
+    size_t byte_index = i >> 3;
+    size_t bit_index = i & 0x7; // Amount to left shift to get bit in MSB
+    if (bit_index == 0) {
+        return buf[byte_index];
+    }
+    return (buf[byte_index] << bit_index) | (buf[byte_index + 1] >> (8 - bit_index));
+}
+
+static inline void bmp_write_byte(uint8_t* buf, size_t i, uint8_t byte) {
+    size_t byte_index = i >> 3;
+    size_t bit_index = i & 0x7; // Amount to left shift to get bit in MSB
+    if (bit_index == 0) {
+        buf[byte_index] = byte;
+        return;
+    }
+    buf[byte_index] = (buf[byte_index] & (0xFF << (8 - bit_index))) | (byte >> bit_index);
+    buf[byte_index + 1] = (buf[byte_index + 1] & (0xFF >> bit_index)) | (byte << (8 - bit_index));
+}
+
+void bmp_swap(uint8_t* buf, size_t start_1, size_t start_2, size_t len) {
+    while ((len & 0x7) != 0) {
+        uint8_t bit_1 = bmp_read_bit(buf, start_1);
+        uint8_t bit_2 = bmp_read_bit(buf, start_2);
+        if (bit_1 != bit_2) {
+            bmp_write_bit(buf, start_1, bit_2);
+            bmp_write_bit(buf, start_2, bit_1);
+        }
+
+        start_1++;
+        start_2++;
+        len--;
+    }
+
+    while (len != 0) {
+        uint8_t byte_1 = bmp_read_byte(buf, start_1);
+        uint8_t byte_2 = bmp_read_byte(buf, start_2);
+        if (byte_1 != byte_2) {
+            bmp_write_byte(buf, start_1, byte_2);
+            bmp_write_byte(buf, start_2, byte_1);
+        }
+
+        start_1 += 8;
+        start_2 += 8;
+        len -= 8;
+    }
+}
+
 int bmp_isempty(uint8_t* buf, size_t buflen) {
     uint8_t* bufend = buf + buflen;
     while (buf < bufend) {

third_party/tcplp/lib/bitmap.h

@@ -57,6 +57,11 @@ void bmp_clrrange(uint8_t* buf, size_t start, size_t len);
    which case it returns exactly the number of set bits it found. */
 size_t bmp_countset(uint8_t* buf, size_t buflen, size_t start, size_t limit);
 
+/* Swaps two non-overlapping regions of the bitmap. START_1 is the index of
+   the first region, START_2 is the index of the secoind region, and LEN is
+   the length of each region, in bits. */
+void bmp_swap(uint8_t* buf, size_t start_1, size_t start_2, size_t len);
+
 /* Returns 1 if the bitmap is all zeros, and 0 otherwise. */
 int bmp_isempty(uint8_t* buf, size_t buflen);
 

third_party/tcplp/lib/cbuf.c

@@ -177,6 +177,95 @@ size_t cbuf_pop(struct cbufhead* chdr, size_t numbytes) {
     return numbytes;
 }
 
+static void cbuf_swap(struct cbufhead* chdr, uint8_t* bitmap, size_t start_1, size_t start_2, size_t length) {
+    size_t i;
+
+    /* Swap the data regions. */
+    for (i = 0; i != length; i++) {
+        uint8_t temp = chdr->buf[start_1 + i];
+        chdr->buf[start_1 + i] = chdr->buf[start_2 + i];
+        chdr->buf[start_2 + i] = temp;
+    }
+
+    /* Swap the bitmaps. */
+    if (bitmap) {
+        bmp_swap(bitmap, start_1, start_2, length);
+    }
+}
+
+void cbuf_contiguify(struct cbufhead* chdr, uint8_t* bitmap) {
+    /*
+     * We treat contiguify as a special case of rotation. In principle, we
+     * could make this more efficient by inspecting R_INDEX, W_INDEX, and the
+     * bitmap to only move around in-sequence data and buffered out-of-sequence
+     * data, while ignoring the other bytes in the circular buffer. We leave
+     * this as an optimization to implement if/when it becomes necessary.
+     *
+     * The rotation algorithm is recursive. It is parameterized by three
+     * arguments. START_IDX is the index of the first element of the subarray
+     * that is being rotated. END_IDX is one plus the index of the last element
+     * of the subarray that is being rotated. MOVE_TO_START_IDX is the index of
+     * the element that should be located at START_IDX after the rotation.
+     *
+     * The algorithm is as follows. First, identify the largest block of data
+     * starting at MOVE_TO_START_IDX that can be swapped with data starting at
+     * START_IDX. If MOVE_TO_START_IDX is right at the midpoint of the array,
+     * then we're done. If it isn't, then we can treat the block of data that
+     * was just swapped to the beginning of the array as "done", and then
+     * complete the rotation by recursively rotating the rest of the array.
+     *
+     * Here's an example. Suppose that the array is "1 2 3 4 5 6 7 8 9" and
+     * MOVE_TO_START_IDX is the index of the element "3". First, we swap "1 2"
+     * AND "3 4" to get "3 4 1 2 5 6 7 8 9". Then, we recursively rotate the
+     * subarray "1 2 5 6 7 8 9", with MOVE_TO_START_IDX being the index of the
+     * element "5". The final array is "3 4 5 6 7 8 9 1 2".
+     *
+     * Here's another example. Suppose that the array is "1 2 3 4 5 6 7 8 9"
+     * and MOVE_TO_START_IDX is the index of the element "6". First, we swap
+     * "1 2 3 4" and "6 7 8 9" to get "6 7 8 9 5 1 2 3 4". Then, we recursively
+     * rotate the subarray "5 1 2 3 4", with MOVE_TO_START_IDX being the index
+     * of the element "1". The final array is "6 7 8 9 1 2 3 4 5".
+     *
+     * In order for this to work, it's important that the blocks that we
+     * choose are maximally large. If, in the first example, we swap only the
+     * elements "1" and "3", then the algorithm won't work. Note that "1 2" and
+     * "3 4" corresponds to maximally large blocks because if we make the
+     * blocks any bigger, they would overlap (e.g., "1 2 3" and "3 4 5"). In
+     * the second example, the block "6 7 8 9" is maximally large because we
+     * reach the end of the subarray.
+     *
+     * The algorithm above is tail-recursive (i.e., there's no more work to do
+     * after recursively rotating the subarray), so we write it as a while
+     * loop below. Each iteration of the while loop identifies the blocks to
+     * swap, swaps the blocks, and then sets up the indices such that the
+     * next iteration of the loop rotates the appropriate subarray.
+     *
+     * The performance of the algorithm is linear in the length of the array,
+     * with constant space overhead.
+     */
+    size_t start_idx = 0;
+    const size_t end_idx = chdr->size;
+    size_t move_to_start_idx = chdr->r_index;
+
+    /* Invariant: start_idx <= move_to_start_idx <= end_idx */
+    while (start_idx < move_to_start_idx && move_to_start_idx < end_idx) {
+        size_t distance_from_start = move_to_start_idx - start_idx;
+        size_t distance_to_end = end_idx - move_to_start_idx;
+        if (distance_from_start <= distance_to_end) {
+            cbuf_swap(chdr, bitmap, start_idx, move_to_start_idx, distance_from_start);
+            start_idx = move_to_start_idx;
+            move_to_start_idx = move_to_start_idx + distance_from_start;
+        } else {
+            cbuf_swap(chdr, bitmap, start_idx, move_to_start_idx, distance_to_end);
+            start_idx = start_idx + distance_to_end;
+            // move_to_start_idx does not change
+        }
+    }
+
+    /* Finally, fix up the indices. */
+    chdr->r_index = 0;
+}
+
 void cbuf_reference(const struct cbufhead* chdr, otLinkedBuffer* first, otLinkedBuffer* second) {
     size_t until_end = chdr->size - chdr->r_index;
     if (chdr->used <= until_end) {

third_party/tcplp/lib/cbuf.h

@@ -64,7 +64,7 @@ void cbuf_copy_from_message(void* arr, size_t arr_offset, const void* buffer, si
 /* Writes data to the back of the circular buffer using the specified copier. */
 size_t cbuf_write(struct cbufhead* chdr, const void* data, size_t data_offset, size_t data_len, cbuf_copier_t copy_from);
 
-/* Reads data from the front ofthe circular buffer using the specified copier. */
+/* Reads data from the front of the circular buffer using the specified copier. */
 size_t cbuf_read(struct cbufhead* chdr, void* data, size_t data_offset, size_t numbytes, int pop, cbuf_copier_t copy_into);
 
 /* Reads data at the specified offset, in bytes, from the front of the circular buffer using the specified copier. */
@@ -85,12 +85,15 @@ size_t cbuf_size(struct cbufhead* chdr);
 /* Returns true if the circular buffer is empty, and false if it is not empty. */
 bool cbuf_empty(struct cbufhead* chdr);
 
+/* Rotates the circular buffer's data so that the "used" portion begins at the beginning of the buffer. */
+void cbuf_contiguify(struct cbufhead* chdr, uint8_t* bitmap);
+
 /* Populates the provided otLinkedBuffers to reference the data currently in the circular buffer. */
 void cbuf_reference(const struct cbufhead* chdr, struct otLinkedBuffer* first, struct otLinkedBuffer* second);
 
 /* Writes DATA at the end of the circular buffer without making it available for
    reading. This data is said to be "out-of-sequence". OFFSET is position at
-   which to write these bytes, relative to the positoin where cbuf_write would
+   which to write these bytes, relative to the position where cbuf_write would
    write them. Each bit in the BITMAP corresponds to a byte in the circular
    buffer; the bits corresponding to the bytes containing the newly written
    data are set. The index of the first byte written is stored into FIRSTINDEX,