Add vstr_ins and vstr_cut_out; improve stmhal readline.

1 files changed, 57 insertions, 3 deletions

diff --git a/py/vstr.c b/py/vstr.c
index 6630ac40a..90c3c06e2 100644
--- a/py/vstr.c
+++ b/py/vstr.c
@@ -159,12 +159,12 @@ char *vstr_add_len(vstr_t *vstr, int len) {
     return buf;
 }
 
-void vstr_add_byte(vstr_t *vstr, byte v) {
+void vstr_add_byte(vstr_t *vstr, byte b) {
     byte *buf = (byte*)vstr_add_len(vstr, 1);
     if (buf == NULL) {
         return;
     }
-    buf[0] = v;
+    buf[0] = b;
 }
 
 void vstr_add_char(vstr_t *vstr, unichar c) {
@@ -214,7 +214,48 @@ void vstr_add_le32(vstr_t *vstr, unsigned int v) {
 }
 */
 
-void vstr_cut_tail(vstr_t *vstr, int len) {
+char *vstr_ins_blank_bytes(vstr_t *vstr, uint byte_pos, uint byte_len) {
+    if (vstr->had_error) {
+        return NULL;
+    }
+    uint l = vstr->len;
+    if (byte_pos > l) {
+        byte_pos = l;
+    }
+    if (byte_len > 0) {
+        // ensure room for the new bytes
+        if (!vstr_ensure_extra(vstr, byte_len)) {
+            return NULL;
+        }
+        // copy up the string to make room for the new bytes
+        memmove(vstr->buf + l - 1 + byte_len, vstr->buf + l - 1, l - byte_pos);
+        // increase the length
+        vstr->len += byte_len;
+        vstr->buf[vstr->len] = 0;
+    }
+    return vstr->buf + byte_pos;
+}
+
+void vstr_ins_byte(vstr_t *vstr, uint byte_pos, byte b) {
+    char *s = vstr_ins_blank_bytes(vstr, byte_pos, 1);
+    if (s != NULL) {
+        *s = b;
+    }
+}
+
+void vstr_ins_char(vstr_t *vstr, uint pos, unichar chr) {
+    // TODO UNICODE
+    char *s = vstr_ins_blank_bytes(vstr, pos, 1);
+    if (s != NULL) {
+        *s = chr;
+    }
+}
+
+void vstr_cut_head_bytes(vstr_t *vstr, uint bytes_to_cut) {
+    vstr_cut_out_bytes(vstr, 0, bytes_to_cut);
+}
+
+void vstr_cut_tail_bytes(vstr_t *vstr, uint len) {
     if (vstr->had_error) {
         return;
     }
@@ -226,6 +267,19 @@ void vstr_cut_tail(vstr_t *vstr, int len) {
     vstr->buf[vstr->len] = 0;
 }
 
+void vstr_cut_out_bytes(vstr_t *vstr, uint byte_pos, uint bytes_to_cut) {
+    if (vstr->had_error || byte_pos >= vstr->len) {
+        return;
+    } else if (byte_pos + bytes_to_cut >= vstr->len) {
+        vstr->len = byte_pos;
+        vstr->buf[vstr->len] = 0;
+    } else {
+        // move includes +1 for null byte at the end
+        memmove(vstr->buf + byte_pos, vstr->buf + byte_pos + bytes_to_cut, vstr->len - byte_pos - bytes_to_cut + 1);
+        vstr->len -= bytes_to_cut;
+    }
+}
+
 void vstr_printf(vstr_t *vstr, const char *fmt, ...) {
     va_list ap;
     va_start(ap, fmt);