firmware/common/structec.c

   1 /***************************************************************************
   2  *             __________               __   ___.
   3  *   Open      \______   \ ____   ____ |  | _\_ |__   _______  ___
   4  *   Source     |       _//  _ \_/ ___\|  |/ /| __ \ /  _ \  \/  /
   5  *   Jukebox    |    |   (  <_> )  \___|    < | \_\ (  <_> > <  <
   6  *   Firmware   |____|_  /\____/ \___  >__|_ \|___  /\____/__/\_ \
   7  *                     \/            \/     \/    \/            \/
   8  * $Id$
   9  *
  10  * Copyright (C) 2007 by Miika Pekkarinen
  11  *
  12  * All files in this archive are subject to the GNU General Public License.
  13  * See the file COPYING in the source tree root for full license agreement.
  14  *
  15  * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
  16  * KIND, either express or implied.
  17  *
  18  ****************************************************************************/
  19
  20 #include <ctype.h>
  21 #include <string.h>
  22 #include <inttypes.h>
  23 #include "structec.h"
  24 #include "system.h"
  25 #include "file.h"
  26
  27 #define MAX_STRUCT_SIZE 128
  28
  29 /**
  30  * Convert the struct endianess with the instructions provided.
  31  *
  32  * For example:
  33  * struct test {
  34  *     long par1;
  35  *     short par2;
  36  *     short par3;
  37  * };
  38  *
  39  * structec_convert(instance_of_test, "lss", sizeof(struct test), true);
  40  *
  41  * Structures to be converted must be properly padded.
  42  *
  43  * @param structure Pointer to the struct being converted.
  44  * @param ecinst Instructions how to do the endianess conversion.
  45  * @param count Number of structures to write
  46  * @param enable Conversion is not made unless this is true.
  47  */
  48 void structec_convert(void *structure, const char *ecinst,
  49                       long count, bool enable)
  50 {
  51     const char *ecinst_ring = ecinst;
  52     char *buf = (char *)structure;
  53
  54     if (!enable)
  55         return;
  56
  57     while (count > 0)
  58     {
  59         switch (*ecinst_ring)
  60         {
  61             /* Swap nothing. */
  62             case 'c':
  63             {
  64                 buf++;
  65                 break;
  66             }
  67
  68             /* Swap 2 bytes. */
  69             case 's':
  70             {
  71                 uint16_t *data = (uint16_t *)buf;
  72                 *data = swap16(*data);
  73                 buf += 2;
  74                 break;
  75             }
  76
  77             /* Swap 4 bytes. */
  78             case 'l':
  79             {
  80                 uint32_t *data = (uint32_t *)buf;
  81                 *data = swap32(*data);
  82                 buf += 4;
  83                 break;
  84             }
  85
  86             /* Skip N bytes, idea taken from metadata.c */
  87             default:
  88             {
  89                 if (isdigit(*ecinst_ring))
  90                     buf += (*ecinst_ring - '0');
  91
  92                 break;
  93             }
  94         }
  95
  96         ecinst_ring++;
  97         if (*ecinst_ring == '\0')
  98         {
  99             ecinst_ring = ecinst;
 100             count--;
 101         }
 102     }
 103 }
 104
 105 /**
 106  * Determines the size of a struct in bytes by using endianess correction
 107  * string format.
 108  *
 109  * @param ecinst endianess correction string.
 110  * @return length of the struct in bytes.
 111  */
 112 size_t structec_size(const char *ecinst)
 113 {
 114     size_t size = 0;
 115
 116     do
 117     {
 118         switch (*ecinst)
 119         {
 120             case 'c': size += 1; break;
 121             case 's': size += 2; break;
 122             case 'l': size += 4; break;
 123             default:
 124                 if (isdigit(*ecinst))
 125                     size += (*ecinst - '0');
 126         }
 127     } while (*(++ecinst) != '\0');
 128
 129     return size;
 130 }
 131
 132 /**
 133  * Reads endianess corrected structure members from the given file.
 134  *
 135  * @param fd file descriptor of the file being read.
 136  * @param buf endianess corrected data is placed here.
 137  * @param scount the number of struct members to read.
 138  * @param ecinst endianess correction string.
 139  * @param ec if true, endianess correction is enabled.
 140  */
 141 ssize_t ecread(int fd, void *buf, size_t scount, const char *ecinst, bool ec)
 142 {
 143     ssize_t ret;
 144     size_t member_size = structec_size(ecinst);
 145
 146     ret = read(fd, buf, scount * member_size);
 147     structec_convert(buf, ecinst, scount, ec);
 148
 149     return ret;
 150 }
 151
 152 /**
 153  * Writes endianess corrected structure members to the given file.
 154  *
 155  * @param fd file descriptor of the file being written to.
 156  * @param buf endianess corrected data is read here.
 157  * @param scount the number of struct members to write.
 158  * @param ecinst endianess correction string.
 159  * @param ec if true, endianess correction is enabled.
 160  */
 161 ssize_t ecwrite(int fd, const void *buf, size_t scount,
 162                 const char *ecinst, bool ec)
 163 {
 164     char tmp[MAX_STRUCT_SIZE];
 165     size_t member_size = structec_size(ecinst);
 166
 167     if (ec)
 168     {
 169         const char *p = (const char *)buf;
 170         int maxamount = (int)(MAX_STRUCT_SIZE / member_size);
 171         int i;
 172
 173         for (i = 0; i < (long)scount; i += maxamount)
 174         {
 175             long amount = MIN((int)scount-i, maxamount);
 176
 177             memcpy(tmp, p, member_size * amount);
 178             structec_convert(tmp, ecinst, amount, true);
 179             write(fd, tmp, amount * member_size);
 180             p += member_size * amount;
 181         }
 182
 183         return scount * member_size;
 184     }
 185
 186     return write(fd, buf, scount * member_size);
 187 }
 188