add memccpy.c

Not sure if this is worth the added bin size yet but I will
see where I can use it to try and make it worth it

Change-Id: Icc299d3986172ff224a14be48da3bf065d728a66

6 files changed, 139 insertions, 5 deletions

diff --git a/apps/debug_menu.c b/apps/debug_menu.c
index 87a41920b9..0b0bc8fc2b 100644
--- a/apps/debug_menu.c
+++ b/apps/debug_menu.c
@@ -1627,7 +1627,9 @@ static int ata_smart_attr_to_string(
             if (len >= name_sz) len = name_sz-1;
             slen += len;
         }
-        snprintf(str+slen, size-slen, "%s", buf);
+
+        if (!memccpy (str+slen, buf, '\0', size-slen))
+            (str+slen)[size-slen - 1] = '\0';
     }
 
     return 1; /* ok */
diff --git a/apps/playlist_viewer.c b/apps/playlist_viewer.c
index d2774e67b0..f1f8954113 100644
--- a/apps/playlist_viewer.c
+++ b/apps/playlist_viewer.c
@@ -516,7 +516,10 @@ static enum pv_onplay_result open_with(const struct playlist_entry *current_trac
 {
     char selected_track[MAX_PATH];
     close_playlist_viewer();
-    snprintf(selected_track, sizeof(selected_track), "%s", current_track->name);
+
+    if (!memccpy (selected_track, current_track->name, '\0', sizeof(selected_track)))
+        selected_track[sizeof(selected_track) - 1] = '\0';
+
 
     return (filetype_list_viewers(selected_track) ==
                 PLUGIN_USB_CONNECTED ? PV_ONPLAY_USB_CLOSED : PV_ONPLAY_CLOSED);
@@ -528,7 +531,9 @@ static enum pv_onplay_result open_pictureflow(const struct playlist_entry *curre
 {
     char selected_track[MAX_PATH];
     close_playlist_viewer();
-    snprintf(selected_track, sizeof(selected_track), "%s", current_track->name);
+
+    if (!memccpy (selected_track, current_track->name, '\0', sizeof(selected_track)))
+        selected_track[sizeof(selected_track) - 1] = '\0';
 
     return (filetype_load_plugin((void *)"pictureflow", selected_track) ==
                 PLUGIN_USB_CONNECTED ? PV_ONPLAY_USB_CLOSED : PV_ONPLAY_CLOSED);
diff --git a/apps/screens.c b/apps/screens.c
index 70374a1f08..9191922c31 100644
--- a/apps/screens.c
+++ b/apps/screens.c
@@ -561,7 +561,10 @@ static const char * id3_get_or_speak_info(int selected_item, void* data,
             case LANG_ID3_COMMENT:
                 if (!id3->comment)
                     return NULL;
-                snprintf(buffer, buffer_len, "%s", id3->comment);
+
+                if (!memccpy (buffer, id3->comment, '\0', buffer_len))
+                    buffer[buffer_len - 1] = '\0';
+
                 val=buffer;
                 if(say_it && val)
                     talk_spell(val, true);
@@ -608,7 +611,10 @@ static const char * id3_get_or_speak_info(int selected_item, void* data,
             case LANG_FORMAT:
                 if (id3->codectype >= AFMT_NUM_CODECS)
                     return NULL;
-                snprintf(buffer, buffer_len, "%s", audio_formats[id3->codectype].label);
+
+                if (!memccpy (buffer, audio_formats[id3->codectype].label, '\0', buffer_len))
+                    buffer[buffer_len - 1] = '\0';
+
                 val=buffer;
                 if(say_it)
                     talk_spell(val, true);
diff --git a/firmware/SOURCES b/firmware/SOURCES
index 76d6cee921..94a986c9f8 100644
--- a/firmware/SOURCES
+++ b/firmware/SOURCES
@@ -262,6 +262,7 @@ libc/ctype.c
 libc/sprintf.c
 #endif
 
+libc/memccpy.c
 libc/memchr.c
 libc/memcmp.c
 
diff --git a/firmware/libc/include/string.h b/firmware/libc/include/string.h
index 9815c62805..4ec2f8dd67 100644
--- a/firmware/libc/include/string.h
+++ b/firmware/libc/include/string.h
@@ -20,6 +20,7 @@ extern "C" {
 _PTR     _EXFUN(memchr,(const _PTR, int, size_t));
 int      _EXFUN(memcmp,(const _PTR, const _PTR, size_t));
 _PTR     _EXFUN(memcpy,(_PTR, const _PTR, size_t));
+_PTR     _EXFUN(memccpy,(_PTR, const _PTR, int, size_t));
 _PTR     _EXFUN(mempcpy,(_PTR, const _PTR, size_t));
 _PTR     _EXFUN(memmove,(_PTR, const _PTR, size_t));
 _PTR     _EXFUN(memset,(_PTR, int, size_t));
diff --git a/firmware/libc/memccpy.c b/firmware/libc/memccpy.c
new file mode 100644
index 0000000000..fa9316616e
--- /dev/null
+++ b/firmware/libc/memccpy.c
@@ -0,0 +1,119 @@
+/*
+FUNCTION
+        <<memccpy>>---copy memory regions with end-token check
+ANSI_SYNOPSIS
+        #include <string.h>
+        void* memccpy(void *restrict <[out]>, const void *restrict <[in]>,
+                      int <[endchar]>, size_t <[n]>);
+TRAD_SYNOPSIS
+        void *memccpy(<[out]>, <[in]>, <[endchar]>, <[n]>
+        void *<[out]>;
+        void *<[in]>;
+    int <[endchar]>;
+        size_t <[n]>;
+DESCRIPTION
+        This function copies up to <[n]> bytes from the memory region
+        pointed to by <[in]> to the memory region pointed to by
+        <[out]>.  If a byte matching the <[endchar]> is encountered,
+    the byte is copied and copying stops.
+        If the regions overlap, the behavior is undefined.
+RETURNS
+        <<memccpy>> returns a pointer to the first byte following the
+    <[endchar]> in the <[out]> region.  If no byte matching
+    <[endchar]> was copied, then <<NULL>> is returned.
+PORTABILITY
+<<memccpy>> is a GNU extension.
+<<memccpy>> requires no supporting OS subroutines.
+    */
+#include <stddef.h>
+#include <string.h>
+#include <limits.h>
+#include "_ansi.h" /* for _DEFUN */
+
+/* Nonzero if either X or Y is not aligned on a "long" boundary.  */
+#define ROCKBOX_UNALIGNED(X, Y) \
+  (((long)X & (sizeof (long) - 1)) | ((long)Y & (sizeof (long) - 1)))
+/* How many bytes are copied each iteration of the word copy loop.  */
+#define LITTLEBLOCKSIZE (sizeof (long))
+/* Threshhold for punting to the byte copier.  */
+#define TOO_SMALL(LEN)  ((LEN) < LITTLEBLOCKSIZE)
+/* Macros for detecting endchar */
+#if LONG_MAX == 2147483647L
+#define DETECTNULL(X) (((X) - 0x01010101) & ~(X) & 0x80808080)
+#else
+#if LONG_MAX == 9223372036854775807L
+/* Nonzero if X (a long int) contains a NULL byte. */
+#define DETECTNULL(X) (((X) - 0x0101010101010101) & ~(X) & 0x8080808080808080)
+#else
+#error long int is not a 32bit or 64bit type.
+#endif
+#endif
+_PTR
+_DEFUN (memccpy, (dst0, src0, endchar, len0),
+    _PTR __restrict dst0 _AND
+    _CONST _PTR __restrict src0 _AND
+    int endchar0 _AND
+    size_t len0)
+{
+#if defined(PREFER_SIZE_OVER_SPEED) || defined(__OPTIMIZE_SIZE__)
+  _PTR ptr = NULL;
+  char *dst = (char *) dst0;
+  char *src = (char *) src0;
+  char endchar = endchar0 & 0xff;
+  while (len0--)
+    {
+      if ((*dst++ = *src++) == endchar)
+        {
+          ptr = dst;
+          break;
+        }
+    }
+  return ptr;
+#else
+  _PTR ptr = NULL;
+  char *dst = dst0;
+  _CONST char *src = src0;
+  long *aligned_dst;
+  _CONST long *aligned_src;
+  char endchar = endchar0 & 0xff;
+  /* If the size is small, or either SRC or DST is unaligned,
+     then punt into the byte copy loop.  This should be rare.  */
+  if (!TOO_SMALL(len0) && !ROCKBOX_UNALIGNED (src, dst))
+    {
+      unsigned int i;
+      unsigned long mask = 0;
+      aligned_dst = (long*)dst;
+      aligned_src = (long*)src;
+      /* The fast code reads the ASCII one word at a time and only
+         performs the bytewise search on word-sized segments if they
+         contain the search character, which is detected by XORing
+         the word-sized segment with a word-sized block of the search
+         character and then detecting for the presence of NULL in the
+         result.  */
+      for (i = 0; i < LITTLEBLOCKSIZE; i++)
+        mask = (mask << 8) + endchar;
+      /* Copy one long word at a time if possible.  */
+      while (len0 >= LITTLEBLOCKSIZE)
+        {
+          unsigned long buffer = (unsigned long)(*aligned_src);
+          buffer ^=  mask;
+          if (DETECTNULL (buffer))
+            break; /* endchar is found, go byte by byte from here */
+          *aligned_dst++ = *aligned_src++;
+          len0 -= LITTLEBLOCKSIZE;
+        }
+       /* Pick up any residual with a byte copier.  */
+      dst = (char*)aligned_dst;
+      src = (char*)aligned_src;
+    }
+  while (len0--)
+    {
+      if ((*dst++ = *src++) == endchar)
+        {
+          ptr = dst;
+          break;
+        }
+    }
+  return ptr;
+#endif /* not PREFER_SIZE_OVER_SPEED */
+}