swab函数 (#53)

* swab函数

Signed-off-by: Yuqia <15287042733@163.com>

* 修正格式

Signed-off-by: Yuqia <15287042733@163.com>
Co-authored-by: fslongjin <longjin@RinGoTek.cn>

kernel/common/unistd.h

@@ -26,3 +26,11 @@ pid_t fork(void);
  * @return pid_t
  */
 pid_t vfork(void);
+
+/**
+ * @brief  交换n字节
+ *  @param src  源地址
+ *  @param dest  目的地址
+ * @param nbytes  交换字节数
+ */
+void swab(void *restrict src, void *restrict dest, ssize_t nbytes);

kernel/lib/unistd.c

@@ -1,4 +1,5 @@
 #include <common/unistd.h>
+#include <common/glib.h>
 
 /**
  * @brief fork当前进程
@@ -19,3 +20,20 @@ pid_t vfork(void)
 {
     return (pid_t)enter_syscall_int(SYS_VFORK, 0, 0, 0, 0, 0, 0, 0, 0);
 }
+
+void swab(void *restrict src, void *restrict dest, ssize_t nbytes)
+{
+    unsigned char buf[32];
+    char *_src = src;
+    char *_dest = dest;
+    uint32_t transfer;
+    for (; nbytes > 0; nbytes -= transfer)
+    {
+        transfer = (nbytes > 32) ? 32 : nbytes;
+        memcpy(buf, _src, transfer);
+        memcpy(_src, _dest, transfer);
+        memcpy(_dest, buf, transfer);
+        _src += transfer;
+        _dest += transfer;
+    }
+}

user/libs/libc/string.h

@@ -48,4 +48,32 @@ char* strcpy(char* dst, const char* src);
  */
 char *strcat(char *dest, const char *src);
 
-#define memcpy(dst, src, n) __builtin_memcpy(dst, src, n)
+/**
+ * @brief 内存拷贝函数
+ *
+ * @param dst 目标数组
+ * @param src 源数组
+ * @param Num 字节数
+ * @return void*
+ */
+static void *memcpy(void *dst, const void *src, long Num)
+{
+    int d0 = 0, d1 = 0, d2 = 0;
+    __asm__ __volatile__("cld	\n\t"
+                         "rep	\n\t"
+                         "movsq	\n\t"
+                         "testb	$4,%b4	\n\t"
+                         "je	1f	\n\t"
+                         "movsl	\n\t"
+                         "1:\ttestb	$2,%b4	\n\t"
+                         "je	2f	\n\t"
+                         "movsw	\n\t"
+                         "2:\ttestb	$1,%b4	\n\t"
+                         "je	3f	\n\t"
+                         "movsb	\n\t"
+                         "3:	\n\t"
+                         : "=&c"(d0), "=&D"(d1), "=&S"(d2)
+                         : "0"(Num / 8), "q"(Num), "1"(dst), "2"(src)
+                         : "memory");
+    return dst;
+}

user/libs/libc/unistd.c

@@ -3,6 +3,7 @@
 #include <libc/errno.h>
 #include <libc/stdio.h>
 #include <libc/stddef.h>
+#include <libc/string.h>
 
 /**
  * @brief 关闭文件接口
@@ -158,3 +159,26 @@ int rmdir(const char *path)
 {
     return syscall_invoke(SYS_RMDIR, (uint64_t)path, 0, 0, 0, 0, 0, 0, 0);
 }
+
+/**
+ * @brief  交换n字节
+ *  @param src  源地址
+ *  @param dest  目的地址
+ * @param nbytes  交换字节数
+ */
+void swab(void *restrict src, void *restrict dest, ssize_t nbytes)
+{
+    unsigned char buf[32];
+    char *_src = src;
+    char *_dest = dest;
+    uint32_t transfer;
+    for (; nbytes > 0; nbytes -= transfer)
+    {
+        transfer = (nbytes > 32) ? 32 : nbytes;
+        memcpy(buf, _src, transfer);
+        memcpy(_src, _dest, transfer);
+        memcpy(_dest, buf, transfer);
+        _src += transfer;
+        _dest += transfer;
+    }
+}

user/libs/libc/unistd.h

@@ -105,3 +105,11 @@ extern int usleep(useconds_t usec);
  * @return int 错误码
  */
 int rmdir(const char *path);
+
+/**
+ * @brief  交换n字节
+ *  @param src  源地址
+ *  @param dest  目的地址
+ * @param nbytes  交换字节数
+ */
+void swab(void *restrict src, void *restrict dest, ssize_t nbytes);