helper: Make unhexify() robust on invalid data

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

diff --git a/src/helper/binarybuffer.c b/src/helper/binarybuffer.c
index b3e44916aa4d44de39f39750226d2bb9c5bb4462..26aa8ccede30ce7f236da74ecbaf9aadae6095d8 100644 (file)
--- a/src/helper/binarybuffer.c
+++ b/src/helper/binarybuffer.c
@@ -16,9 +16,7 @@
  *   GNU General Public License for more details.                          *
  *                                                                         *
  *   You should have received a copy of the GNU General Public License     *
- *   along with this program; if not, write to the                         *
- *   Free Software Foundation, Inc.,                                       *
- *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
+ *   along with this program.  If not, see <http://www.gnu.org/licenses/>. *
  ***************************************************************************/
 
 #ifdef HAVE_CONFIG_H
@@ -148,7 +146,7 @@ void *buf_set_buf(const void *_src, unsigned src_start,
        if ((sq == 0) && (dq == 0) &&  (lq == 0)) {
                for (i = 0; i < lb; i++)
                        *dst++ = *src++;
-               return (uint8_t *)_dst;
+               return _dst;
        }
 
        /* fallback to slow bit copy */
@@ -167,7 +165,7 @@ void *buf_set_buf(const void *_src, unsigned src_start,
                }
        }
 
-       return (uint8_t *)_dst;
+       return _dst;
 }
 
 uint32_t flip_u32(uint32_t value, unsigned int num)
@@ -231,7 +229,7 @@ char *buf_to_str(const void *_buf, unsigned buf_len, unsigned radix)
                }
        }
 
-       const char *DIGITS = "0123456789ABCDEF";
+       const char * const DIGITS = "0123456789ABCDEF";
        for (unsigned j = 0; j < str_len; j++)
                str[j] = DIGITS[(int)str[j]];
 
@@ -371,17 +369,42 @@ void bit_copy_discard(struct bit_copy_queue *q)
        }
 }
 
-int unhexify(char *bin, const char *hex, int count)
+/**
+ * Convert a string of hexadecimal pairs into its binary
+ * representation.
+ *
+ * @param[out] bin Buffer to store binary representation. The buffer size must
+ *                 be at least @p count.
+ * @param[in] hex String with hexadecimal pairs to convert into its binary
+ *                representation.
+ * @param[in] count Number of hexadecimal pairs to convert.
+ *
+ * @return The number of converted hexadecimal pairs.
+ */
+size_t unhexify(uint8_t *bin, const char *hex, size_t count)
 {
-       int i, tmp;
+       size_t i;
+       char tmp;
 
-       for (i = 0; i < count; i++) {
-               if (sscanf(hex + (2 * i), "%02x", &tmp) != 1)
-                       return i;
-               bin[i] = tmp;
+       if (!bin || !hex)
+               return 0;
+
+       memset(bin, 0, count);
+
+       for (i = 0; i < 2 * count; i++) {
+               if (hex[i] >= 'a' && hex[i] <= 'f')
+                       tmp = hex[i] - 'a' + 10;
+               else if (hex[i] >= 'A' && hex[i] <= 'F')
+                       tmp = hex[i] - 'A' + 10;
+               else if (hex[i] >= '0' && hex[i] <= '9')
+                       tmp = hex[i] - '0';
+               else
+                       return i / 2;
+
+               bin[i / 2] |= tmp << (4 * ((i + 1) % 2));
        }
 
-       return i;
+       return i / 2;
 }
 
 int hexify(char *hex, const char *bin, int count, int out_maxlen)
@@ -397,3 +420,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);
+       }
+}