X-Git-Url: https://review.openocd.org/gitweb?p=openocd.git;a=blobdiff_plain;f=src%2Fhelper%2Fbinarybuffer.c;h=6fe3664a7fa09e1b94d1bf31623551e2fdad3a63;hp=5defcda4c7c9ad835e88b666c09b8d6f0e97e63c;hb=4e79b48e2c7e535ef21178a69788c15b571c72ff;hpb=d19fafc8bdb30974e70bfc5a6ce63e7578b6e3b2;ds=sidebyside

diff --git a/src/helper/binarybuffer.c b/src/helper/binarybuffer.c
index 5defcda4c7..6fe3664a7f 100644
--- a/src/helper/binarybuffer.c
+++ b/src/helper/binarybuffer.c
@@ -397,3 +397,24 @@ int hexify(char *hex, const char *bin, int count, int out_maxlen)
 
 	return cmd_len;
 }
+
+void buffer_shr(void *_buf, unsigned buf_len, unsigned count)
+{
+	unsigned i;
+	unsigned char *buf = _buf;
+	unsigned bytes_to_remove;
+	unsigned shift;
+
+	bytes_to_remove = count / 8;
+	shift = count - (bytes_to_remove * 8);
+
+	for (i = 0; i < (buf_len - 1); i++)
+		buf[i] = (buf[i] >> shift) | ((buf[i+1] << (8 - shift)) & 0xff);
+
+	buf[(buf_len - 1)] = buf[(buf_len - 1)] >> shift;
+
+	if (bytes_to_remove) {
+		memmove(buf, &buf[bytes_to_remove], buf_len - bytes_to_remove);
+		memset(&buf[buf_len - bytes_to_remove], 0, bytes_to_remove);
+	}
+}