Add new target type: OpenRISC

[fw/openocd] / src / helper / binarybuffer.c

diff --git a/src/helper/binarybuffer.c b/src/helper/binarybuffer.c
index 5defcda4c7c9ad835e88b666c09b8d6f0e97e63c..6fe3664a7fa09e1b94d1bf31623551e2fdad3a63 100644 (file)
--- 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);
+       }
+}