KernelLand/Modules/Interfaces/UDI/udi_lib/core/layout.c

   1 /*
   2  * Acess2 UDI Layer
   3  * - By John Hodge (thePowersGang)
   4  *
   5  * udi_lib/core/layout.c
   6  * - udi_layout_t related functions
   7  */
   8 #define DEBUG   1
   9 #include <udi.h>
  10 #include <acess.h>
  11 #include <udi_internal.h>
  12
  13 // === CODE ===
  14 #define alignto(ofs,type) \
  15         ofs + ((sizeof(type)-ofs%sizeof(type)) % sizeof(type));
  16 #define _PUT(type,vtype) do{\
  17         ofs = alignto(ofs,type); \
  18         if(buf){\
  19                 *(type*)(buf+ofs) = va_arg(*values,vtype);\
  20                 LOG(#type" %p %x", buf+ofs, *(type*)(buf+ofs));\
  21         }\
  22         else if(values) \
  23                 va_arg(*values,vtype); \
  24         ofs += sizeof(type); \
  25 }while(0)
  26
  27 size_t _udi_marshal_step(void *buf, size_t cur_ofs, udi_layout_t **layoutp, va_list *values)
  28 {
  29         size_t  ofs = cur_ofs;
  30         udi_layout_t    *layout = *layoutp;
  31         switch(*layout++)
  32         {
  33         case UDI_DL_UBIT8_T:    _PUT(udi_ubit8_t, UDI_VA_UBIT8_T);      break;
  34         case UDI_DL_SBIT8_T:    _PUT(udi_sbit8_t, UDI_VA_SBIT8_T);      break;
  35         case UDI_DL_UBIT16_T:   _PUT(udi_ubit16_t, UDI_VA_UBIT16_T);    break;
  36         case UDI_DL_SBIT16_T:   _PUT(udi_sbit16_t, UDI_VA_SBIT16_T);    break;
  37         case UDI_DL_UBIT32_T:   _PUT(udi_ubit32_t, UDI_VA_UBIT32_T);    break;
  38         case UDI_DL_SBIT32_T:   _PUT(udi_sbit32_t, UDI_VA_SBIT16_T);    break;
  39         case UDI_DL_BOOLEAN_T:  _PUT(udi_boolean_t, UDI_VA_BOOLEAN_T);  break;
  40         case UDI_DL_STATUS_T:   _PUT(udi_status_t, UDI_VA_STATUS_T);    break;
  41
  42         case UDI_DL_INDEX_T:    _PUT(udi_index_t, UDI_VA_INDEX_T);      break;
  43
  44         case UDI_DL_CHANNEL_T:  _PUT(udi_channel_t, UDI_VA_CHANNEL_T);  break;
  45         case UDI_DL_ORIGIN_T:   _PUT(udi_origin_t, UDI_VA_ORIGIN_T);    break;
  46
  47         case UDI_DL_BUF:
  48                 _PUT(udi_buf_t*,UDI_VA_POINTER);
  49                 layout += 3;
  50                 break;
  51         case UDI_DL_CB:
  52                 _PUT(udi_cb_t*,UDI_VA_POINTER);
  53                 break;
  54
  55         default:
  56                 Log_Error("UDI", "_udi_marshal_values - Unknown layout code %i", layout[-1]);
  57                 return 0;
  58         }
  59         *layoutp = layout;
  60         return ofs;
  61 }
  62
  63 size_t _udi_marshal_values(void *buf, udi_layout_t *layout, va_list values)
  64 {
  65         size_t  ofs = 0;
  66         while( *layout != UDI_DL_END )
  67         {
  68                 ofs = _udi_marshal_step(buf, ofs, &layout, &values);
  69                 if( ofs == 0 )
  70                         return 0;
  71         }
  72         return ofs;
  73 }