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