po_hi_messages.c 4.28 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.
 *
jhugues's avatar
jhugues committed
6
 * For more informations, please visit http://assert-project.net/taste
7
 *
jhugues's avatar
jhugues committed
8
 * Copyright (C) 2007-2009 Telecom ParisTech, 2010-2012 ESA & ISAE.
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
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
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
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
185
186
187
188
189
190
191
 */

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

/* Headers from PolyORB-HI-C */

#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);
}

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

void __po_hi_msg_read (__po_hi_msg_t*  msg, 
		       void*           data, 
		       __po_hi_uint32_t len)
{
  memcpy (data, msg->content, 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)
{
  memcpy (dest->content, 
	  src->content, 
	  __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;
}

#ifdef __PO_HI_USE_GIOP
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++)
        {
                udest[tmp] = ufrom[size-tmp];
        }
}
#endif

#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] = ' ';
   
   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