1 files changed, 179 insertions, 0 deletions

diff --git a/firmware/common/structec.c b/firmware/common/structec.c
new file mode 100644
index 0000000000..ec7e1409fd
--- /dev/null
+++ b/firmware/common/structec.c
@@ -0,0 +1,179 @@
+/***************************************************************************
+ *             __________               __   ___.
+ *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
+ *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
+ *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
+ *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
+ *                     \/            \/     \/    \/            \/
+ * $Id$
+ *
+ * Copyright (C) 2007 by Miika Pekkarinen
+ *
+ * All files in this archive are subject to the GNU General Public License.
+ * See the file COPYING in the source tree root for full license agreement.
+ *
+ * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
+ * KIND, either express or implied.
+ *
+ ****************************************************************************/
+
+#include <string.h>
+#include "structec.h"
+#include "system.h"
+#include "file.h"
+
+#define MAX_STRUCT_SIZE 128
+
+/**
+ * Convert the struct endianess with the instructions provided.
+ * 
+ * For example:
+ * struct test {
+ *     long par1;
+ *     short par2;
+ *     short par3;
+ * };
+ * 
+ * structec_convert(instance_of_test, "lss", sizeof(struct test), true);
+ * 
+ * Structures to be converted must be properly padded.
+ * 
+ * @param structure Pointer to the struct being converted.
+ * @param ecinst Instructions how to do the endianess conversion.
+ * @param count Number of structures to write
+ * @param enable Conversion is not made unless this is true.
+ */
+void structec_convert(void *structure, const char *ecinst, 
+                      long count, bool enable)
+{
+    const char *ecinst_ring = ecinst;
+    char *buf = (char *)structure;
+    
+    if (!enable)
+        return;
+    
+    while (count > 0)
+    {
+        switch (*ecinst_ring)
+        {
+            /* Swap nothing. */
+            case 'c':
+            {
+                buf++;
+                break;
+            }
+            
+            /* Swap 2 bytes. */
+            case 's':
+            {
+                unsigned short *data = (unsigned short *)buf;
+                *data = SWAP_16(*data);
+                buf += 2;
+                break;
+            }
+            
+            /* Swap 4 bytes. */
+            case 'l':
+            {
+                unsigned long *data = (unsigned long *)buf;
+                *data = SWAP_32(*data);
+                buf += 4;
+                break;
+            }
+            
+            /* This should be never reached. */
+            default:
+                break;
+        }
+        
+        ecinst_ring++;
+        if (*ecinst_ring == '\0')
+        {
+            ecinst_ring = ecinst;
+            count--;
+        }
+    }
+}
+
+/**
+ * Determines the size of a struct in bytes by using endianess correction
+ * string format.
+ * 
+ * @param ecinst endianess correction string.
+ * @return length of the struct in bytes.
+ */
+size_t structec_size(const char *ecinst)
+{
+    size_t size = 0;
+    
+    do
+    {
+        switch (*ecinst)
+        {
+            case 'c': size += 1; break;
+            case 's': size += 2; break;
+            case 'l': size += 4; break;
+            default: break;
+        }
+    } while (*(++ecinst) != '\0');
+    
+    return size;
+}
+
+/**
+ * Reads endianess corrected structure members from the given file.
+ * 
+ * @param fd file descriptor of the file being read.
+ * @param buf endianess corrected data is placed here.
+ * @param scount the number of struct members to read.
+ * @param ecinst endianess correction string.
+ * @param ec if true, endianess correction is enabled.
+ */
+ssize_t ecread(int fd, void *buf, size_t scount, const char *ecinst, bool ec)
+{
+    ssize_t ret;
+    size_t member_size = structec_size(ecinst);
+    
+    ret = read(fd, buf, scount * member_size);
+    structec_convert(buf, ecinst, scount, ec);
+    
+    return ret;
+}
+
+/**
+ * Writes endianess corrected structure members to the given file.
+ * 
+ * @param fd file descriptor of the file being written to.
+ * @param buf endianess corrected data is read here.
+ * @param scount the number of struct members to write.
+ * @param ecinst endianess correction string.
+ * @param ec if true, endianess correction is enabled.
+ */
+ssize_t ecwrite(int fd, const void *buf, size_t scount, 
+                const char *ecinst, bool ec)
+{
+    char tmp[MAX_STRUCT_SIZE];
+    size_t member_size = structec_size(ecinst);
+    
+    if (ec)
+    {
+        const char *p = (const char *)buf;
+        int maxamount = (int)(MAX_STRUCT_SIZE / member_size);
+        int i;
+        
+        for (i = 0; i < (long)scount; i += maxamount)
+        {
+            long amount = MIN((int)scount-i, maxamount);
+            
+            memcpy(tmp, p, member_size * amount);
+            structec_convert(tmp, ecinst, amount, true);
+            write(fd, tmp, amount * member_size);
+            p += member_size * amount;
+        }
+        
+        return scount * member_size;
+    }
+    
+    return write(fd, buf, scount * member_size);
+}
+