po_hi_messages.c 4.21 KB
Newer Older
1
2
3
4
5
/*
 * This is a part of PolyORB-HI-C distribution, a minimal
 * middleware written for generated code from AADL models.
 * You should use it with the Ocarina toolsuite.
 *
yoogx's avatar
yoogx committed
6
 * For more informations, please visit http://taste.tuxfamily.org/wiki
7
 *
8
 * Copyright (C) 2007-2009 Telecom ParisTech, 2010-2016 ESA & ISAE.
9
10
11
12
13
14
15
16
 */

#include <po_hi_config.h>
#include <po_hi_types.h>
#include <po_hi_messages.h>

/* Headers from PolyORB-HI-C */

17
#if defined (__CYGWIN__) || defined (__MINGW32__) || defined (RTEMS_POSIX) || defined (RTEMS_PURE)
yoogx's avatar
yoogx committed
18
#else
19
#include <xlocale.h>
yoogx's avatar
yoogx committed
20
#endif
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
#include <string.h>

/* Headers from the executive */

#ifdef __PO_HI_DEBUG
#include <stdio.h>
#endif


void __po_hi_msg_reallocate (__po_hi_msg_t* message)
{
  message->length = 0;
  message->flags = 0;
  memset (message->content, 0, __PO_HI_MESSAGES_MAX_SIZE);
}

37
38
void __po_hi_msg_write (__po_hi_msg_t*  msg,
			void*           data,
39
40
41
42
43
44
45
46
47
48
49
50
51
52
			__po_hi_uint32_t len)
{
  memcpy (msg->content, data, len);
  msg->length = len;
}
int __po_hi_msg_length (__po_hi_msg_t* msg)
{
  return (msg->length);
}


void __po_hi_msg_copy (__po_hi_msg_t* dest,
		       __po_hi_msg_t* src)
{
53
54
  memcpy (dest->content,
	  src->content,
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
	  __PO_HI_MESSAGES_MAX_SIZE);
  dest->length = src->length;
}

void __po_hi_msg_append_data (__po_hi_msg_t* msg, void* data, __po_hi_uint32_t length)
{
        memcpy (msg->content + msg->length, data, length);
        msg->length = msg->length + length;
}

void __po_hi_msg_append_msg (__po_hi_msg_t* dest, __po_hi_msg_t* source)
{
        memcpy (&(dest->content[dest->length]), source->content, source->length);
        dest->length = dest->length + source->length;
}

void __po_hi_msg_get_data (void* dest, __po_hi_msg_t* source, __po_hi_uint32_t index, __po_hi_uint32_t size)
{
        memcpy (dest, &(source->content[index]), size);
}

void __po_hi_msg_move (__po_hi_msg_t* msg, __po_hi_uint32_t length)
{
   __po_hi_uint32_t tmp;
   for (tmp=length; tmp < msg->length ; tmp++)
   {
      msg->content[tmp-length] = msg->content[tmp];
   }
   msg->length = msg->length - length;
}

int __po_hi_msg_should_swap (__po_hi_msg_t* msg)
{
#ifdef WORDS_BIGENDIAN
        if (msg->flags == __PO_HI_MESSAGES_CONTENT_LITTLEENDIAN)
        {
                return 1;
        }
#else
        if (msg->flags == __PO_HI_MESSAGES_CONTENT_BIGENDIAN)
        {
                return 1;
        }
#endif
        return 0;
}

void __po_hi_msg_swap_value (void* from, void* dest, __po_hi_uint8_t size)
{
        __po_hi_uint8_t tmp;
        __po_hi_uint8_t* udest;
        __po_hi_uint8_t* ufrom;

        ufrom = (__po_hi_uint8_t*)from;
        udest = (__po_hi_uint8_t*)dest;

        for (tmp=0 ; tmp < size ; tmp++)
        {
yoogx's avatar
yoogx committed
113
                udest[tmp] = ufrom[size-tmp - 1];
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
        }
}

#ifdef __PO_HI_DEBUG
void __po_hi_messages_debug (__po_hi_msg_t* msg)
{
   uint32_t length;
   char Hexa [50];
   char ASCII [17];
   int Index_Hexa = 0;
   int Index_ASCII = 0;
   int i;

   for (i = 0 ; i < 50 ; i++)
     Hexa[i] = ' ';
129

130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
   for (i = 0 ; i < 17 ; i++)
     ASCII[i] = ' ';

   printf ("Length: %u\n", (unsigned int) msg->length);

   for (length = 0 ; length < msg->length ; length++)
   {
     Hexa [Index_Hexa] = ' ';

     if ((msg->content[length]/16) > 9)
       Hexa [Index_Hexa + 1] = 'A' -10 + (msg->content[length] / 16);
     else
       Hexa [Index_Hexa + 1] = '0' + (msg->content[length] / 16);

     if ((msg->content[length] % 16) > 9)
       Hexa [Index_Hexa + 2] = 'A' - 10 + (msg->content[length] % 16);
     else
       Hexa [Index_Hexa + 2] = '0' + (msg->content[length] % 16);

     Index_Hexa += 3;

     if ((msg->content[length] < 32) || (msg->content[length] > 127))
       ASCII [Index_ASCII] = '.';
     else
       ASCII [Index_ASCII] = msg->content[length];

     Index_ASCII++;

     if (Index_Hexa >= 45)
       {
	 Hexa[Index_Hexa] = '\0';
	 ASCII[Index_ASCII] = '\0';
	 printf ("%s  ||  %s\n", Hexa, ASCII);
	 Index_Hexa = 0;
	 Index_ASCII = 0;
	 for (i = 0 ; i < 50 ; i++)
	   Hexa[i] = ' ';

	 for (i = 0 ; i < 17 ; i++)
	   ASCII[i] = ' ';
       }
   }
   if (Index_Hexa > 0)
     {
       for (i = Index_Hexa ; i<44 ; i++)
	 Hexa[i] = ' ';
       Index_Hexa = 45;

       Hexa[Index_Hexa] = '\0';
       ASCII[Index_ASCII] = '\0';
       printf ("%s  ||  %s\n", Hexa, ASCII);
     }

}
#endif