X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fhelper%2Fbinarybuffer.c;h=e789e6ff1b52a1c0282c124483f163dd62dd8d83;hp=3a16cce69bb2b7f66bac21cbd5ce2ebd5fbbeca7;hb=706284a8fd562d5a6e2c1f123f8cb366b7e49e7d;hpb=5ca7cbe2d232fcaff32598af1ef5e962e704c004

diff --git a/src/helper/binarybuffer.c b/src/helper/binarybuffer.c
index 3a16cce69b..e789e6ff1b 100644
--- a/src/helper/binarybuffer.c
+++ b/src/helper/binarybuffer.c
@@ -133,19 +133,34 @@ void* buf_set_buf(const void *_src, unsigned src_start,
 {
 	const uint8_t *src = _src;
 	uint8_t *dst = _dst;
+	unsigned  sb,db,sq,dq;
+
+	sb = src_start / 8;
+	db = dst_start / 8;
+	sq = src_start % 8;
+	dq = dst_start % 8;
 
-	unsigned src_idx = src_start, dst_idx = dst_start;
 	for (unsigned i = 0; i < len; i++)
 	{
-		if (((src[src_idx / 8] >> (src_idx % 8)) & 1) == 1)
-			dst[dst_idx / 8] |= 1 << (dst_idx % 8);
+		if (((*src >> (sq&7)) & 1) == 1)
+			*dst |= 1 << (dq&7);
 		else
-			dst[dst_idx / 8] &= ~(1 << (dst_idx % 8));
-		dst_idx++;
-		src_idx++;
+			*dst &= ~(1 << (dq&7));
+
+		if ( sq++ == 7 )
+		{
+			sq = 0;
+			src++;
+		}
+
+		if ( dq++ == 7 )
+		{
+			dq = 0;
+			dst++;
+		}
 	}
 
-	return dst;
+	return (uint8_t*)_dst;
 }
 
 uint32_t flip_u32(uint32_t value, unsigned int num)