Commit | Line | Data |
---|---|---|
970ed795 | 1 | /////////////////////////////////////////////////////////////////////////////// |
3abe9331 | 2 | // Copyright (c) 2000-2015 Ericsson Telecom AB |
970ed795 EL |
3 | // All rights reserved. This program and the accompanying materials |
4 | // are made available under the terms of the Eclipse Public License v1.0 | |
5 | // which accompanies this distribution, and is available at | |
6 | // http://www.eclipse.org/legal/epl-v10.html | |
7 | /////////////////////////////////////////////////////////////////////////////// | |
8 | #include "Encdec.hh" | |
9 | ||
10 | #include "../common/memory.h" | |
11 | #include "Octetstring.hh" | |
12 | #include "Charstring.hh" | |
13 | #include "String_struct.hh" | |
14 | #include "RAW.hh" | |
15 | #include "Error.hh" | |
16 | #include "Logger.hh" | |
17 | ||
18 | const TTCN_EncDec::error_behavior_t | |
19 | TTCN_EncDec::default_error_behavior[TTCN_EncDec::ET_ALL] = { | |
20 | TTCN_EncDec::EB_ERROR, | |
21 | TTCN_EncDec::EB_ERROR, | |
22 | TTCN_EncDec::EB_ERROR, | |
23 | TTCN_EncDec::EB_WARNING, | |
24 | TTCN_EncDec::EB_ERROR, | |
25 | TTCN_EncDec::EB_WARNING, | |
26 | TTCN_EncDec::EB_ERROR, | |
27 | TTCN_EncDec::EB_WARNING, | |
28 | TTCN_EncDec::EB_WARNING, | |
29 | TTCN_EncDec::EB_ERROR, | |
30 | TTCN_EncDec::EB_ERROR, | |
31 | TTCN_EncDec::EB_ERROR, | |
32 | TTCN_EncDec::EB_ERROR, | |
33 | TTCN_EncDec::EB_ERROR, | |
34 | TTCN_EncDec::EB_WARNING, | |
35 | TTCN_EncDec::EB_WARNING, | |
36 | TTCN_EncDec::EB_WARNING, | |
37 | TTCN_EncDec::EB_ERROR, | |
38 | TTCN_EncDec::EB_ERROR, | |
39 | TTCN_EncDec::EB_ERROR, | |
40 | TTCN_EncDec::EB_ERROR, | |
41 | TTCN_EncDec::EB_IGNORE, | |
42 | TTCN_EncDec::EB_WARNING, | |
43 | TTCN_EncDec::EB_ERROR, | |
44 | TTCN_EncDec::EB_ERROR | |
45 | }; | |
46 | ||
47 | TTCN_EncDec::error_behavior_t | |
48 | TTCN_EncDec::error_behavior[TTCN_EncDec::ET_ALL] = { | |
49 | TTCN_EncDec::EB_ERROR, | |
50 | TTCN_EncDec::EB_ERROR, | |
51 | TTCN_EncDec::EB_ERROR, | |
52 | TTCN_EncDec::EB_WARNING, | |
53 | TTCN_EncDec::EB_ERROR, | |
54 | TTCN_EncDec::EB_WARNING, | |
55 | TTCN_EncDec::EB_ERROR, | |
56 | TTCN_EncDec::EB_WARNING, | |
57 | TTCN_EncDec::EB_WARNING, | |
58 | TTCN_EncDec::EB_ERROR, | |
59 | TTCN_EncDec::EB_ERROR, | |
60 | TTCN_EncDec::EB_ERROR, | |
61 | TTCN_EncDec::EB_ERROR, | |
62 | TTCN_EncDec::EB_ERROR, | |
63 | TTCN_EncDec::EB_WARNING, | |
64 | TTCN_EncDec::EB_WARNING, | |
65 | TTCN_EncDec::EB_WARNING, | |
66 | TTCN_EncDec::EB_ERROR, | |
67 | TTCN_EncDec::EB_ERROR, | |
68 | TTCN_EncDec::EB_ERROR, | |
69 | TTCN_EncDec::EB_ERROR, | |
70 | TTCN_EncDec::EB_IGNORE, | |
71 | TTCN_EncDec::EB_WARNING, | |
72 | TTCN_EncDec::EB_ERROR, | |
73 | TTCN_EncDec::EB_ERROR | |
74 | }; | |
75 | ||
76 | TTCN_EncDec::error_type_t TTCN_EncDec::last_error_type=ET_NONE; | |
77 | char *TTCN_EncDec::error_str=NULL; | |
78 | ||
79 | static TTCN_EncDec::error_type_t& operator++(TTCN_EncDec::error_type_t& eb) | |
80 | { | |
81 | return eb | |
82 | =(eb==TTCN_EncDec::ET_NONE) | |
83 | ?TTCN_EncDec::ET_UNDEF | |
84 | :TTCN_EncDec::error_type_t(eb+1); | |
85 | } | |
86 | ||
87 | void TTCN_EncDec::set_error_behavior(error_type_t p_et, error_behavior_t p_eb) | |
88 | { | |
89 | if(p_et<ET_UNDEF || p_et>ET_ALL || p_eb<EB_DEFAULT || p_eb>EB_IGNORE) | |
90 | TTCN_error("EncDec::set_error_behavior(): Invalid parameter."); | |
91 | if(p_eb==EB_DEFAULT) { | |
92 | if(p_et==ET_ALL) | |
93 | for(error_type_t i=ET_UNDEF; i<ET_ALL; ++i) | |
94 | error_behavior[i]=default_error_behavior[i]; | |
95 | else | |
96 | error_behavior[p_et]=default_error_behavior[p_et]; | |
97 | } | |
98 | else { | |
99 | if(p_et==ET_ALL) | |
100 | for(error_type_t i=ET_UNDEF; i<ET_ALL; ++i) | |
101 | error_behavior[i]=p_eb; | |
102 | else | |
103 | error_behavior[p_et]=p_eb; | |
104 | } | |
105 | } | |
106 | ||
107 | TTCN_EncDec::error_behavior_t | |
108 | TTCN_EncDec::get_error_behavior(error_type_t p_et) | |
109 | { | |
110 | if(p_et<ET_UNDEF || p_et>=ET_ALL) | |
111 | TTCN_error("EncDec::get_error_behavior(): Invalid parameter."); | |
112 | return error_behavior[p_et]; | |
113 | } | |
114 | ||
115 | TTCN_EncDec::error_behavior_t | |
116 | TTCN_EncDec::get_default_error_behavior(error_type_t p_et) | |
117 | { | |
118 | if(p_et<ET_UNDEF || p_et>=ET_ALL) | |
119 | TTCN_error("EncDec::get_error_behavior(): Invalid parameter."); | |
120 | return default_error_behavior[p_et]; | |
121 | } | |
122 | ||
123 | void TTCN_EncDec::clear_error() | |
124 | { | |
125 | last_error_type=ET_NONE; | |
126 | Free(error_str); error_str=NULL; | |
127 | } | |
128 | ||
129 | void TTCN_EncDec::error(error_type_t p_et, char *msg) | |
130 | { | |
131 | last_error_type=p_et; | |
132 | Free(error_str); | |
133 | error_str=msg; | |
134 | if (p_et >= ET_UNDEF && p_et < ET_ALL) { | |
135 | switch(error_behavior[p_et]) { | |
136 | case TTCN_EncDec::EB_ERROR: | |
137 | TTCN_error("%s", error_str); | |
138 | case TTCN_EncDec::EB_WARNING: | |
139 | TTCN_warning("%s", error_str); | |
140 | break; | |
141 | default: | |
142 | break; | |
143 | } // switch | |
144 | } | |
145 | } | |
146 | ||
147 | TTCN_EncDec_ErrorContext *TTCN_EncDec_ErrorContext::head=NULL; | |
148 | TTCN_EncDec_ErrorContext *TTCN_EncDec_ErrorContext::tail=NULL; | |
149 | ||
150 | TTCN_EncDec_ErrorContext::TTCN_EncDec_ErrorContext() | |
151 | { | |
152 | msg=NULL; | |
153 | if(!head) head=this; | |
154 | if(tail) tail->next=this; | |
155 | prev=tail; | |
156 | next=0; | |
157 | tail=this; | |
158 | } | |
159 | ||
160 | TTCN_EncDec_ErrorContext::TTCN_EncDec_ErrorContext(const char *fmt, ...) | |
161 | { | |
162 | va_list args; | |
163 | va_start(args, fmt); | |
164 | msg=mprintf_va_list(fmt, args); | |
165 | va_end(args); | |
166 | if(!head) head=this; | |
167 | if(tail) tail->next=this; | |
168 | prev=tail; | |
169 | next=0; | |
170 | tail=this; | |
171 | } | |
172 | ||
173 | TTCN_EncDec_ErrorContext::~TTCN_EncDec_ErrorContext() | |
174 | { | |
175 | Free(msg); | |
176 | if(tail!=this) | |
177 | TTCN_error("Internal error:" | |
178 | " TTCN_EncDec_ErrorContext::~TTCN_EncDec_ErrorContext()"); | |
179 | if(prev) prev->next=NULL; | |
180 | else head=NULL; | |
181 | tail=prev; | |
182 | } | |
183 | ||
184 | void TTCN_EncDec_ErrorContext::set_msg(const char *fmt, ...) | |
185 | { | |
186 | Free(msg); | |
187 | va_list args; | |
188 | va_start(args, fmt); | |
189 | msg=mprintf_va_list(fmt, args); | |
190 | va_end(args); | |
191 | } | |
192 | ||
193 | void TTCN_EncDec_ErrorContext::error(TTCN_EncDec::error_type_t p_et, | |
194 | const char *fmt, ...) | |
195 | { | |
196 | char *err_msg=NULL; | |
197 | for(TTCN_EncDec_ErrorContext *p=head; p!=NULL; p=p->next) | |
198 | err_msg=mputstr(err_msg, p->msg); | |
199 | va_list parameters; | |
200 | va_start(parameters, fmt); | |
201 | err_msg=mputprintf_va_list(err_msg, fmt, parameters); | |
202 | va_end(parameters); | |
203 | TTCN_EncDec::error(p_et, err_msg); | |
204 | } | |
205 | ||
206 | void TTCN_EncDec_ErrorContext::error_internal(const char *fmt, ...) | |
207 | { | |
208 | char *err_msg=mcopystr("Internal error: "); | |
209 | for(TTCN_EncDec_ErrorContext *p=head; p!=NULL; p=p->next) | |
210 | err_msg=mputstr(err_msg, p->msg); | |
211 | va_list parameters; | |
212 | va_start(parameters, fmt); | |
213 | err_msg=mputprintf_va_list(err_msg, fmt, parameters); | |
214 | va_end(parameters); | |
215 | TTCN_EncDec::error(TTCN_EncDec::ET_INTERNAL, err_msg); | |
216 | TTCN_error("%s", TTCN_EncDec::get_error_str()); | |
217 | } | |
218 | ||
219 | void TTCN_EncDec_ErrorContext::warning(const char *fmt, ...) | |
220 | { | |
221 | char *warn_msg=NULL; | |
222 | for(TTCN_EncDec_ErrorContext *p=head; p!=NULL; p=p->next) | |
223 | warn_msg=mputstr(warn_msg, p->msg); | |
224 | va_list parameters; | |
225 | va_start(parameters, fmt); | |
226 | warn_msg=mputprintf_va_list(warn_msg, fmt, parameters); | |
227 | va_end(parameters); | |
228 | TTCN_warning("%s", warn_msg); | |
229 | Free(warn_msg); | |
230 | } | |
231 | ||
232 | #define INITIAL_SIZE 1024 | |
233 | #define MEMORY_SIZE(n) (sizeof(buffer_struct) - sizeof(int) + (n)) | |
234 | ||
235 | void TTCN_Buffer::reset_buffer() | |
236 | { | |
237 | buf_pos = 0; | |
238 | bit_pos = 0; | |
239 | last_bit_pos = 0; | |
240 | last_bit_bitpos = 0; | |
241 | start_of_ext_bit = 0; | |
242 | last_bit = FALSE; | |
243 | current_bitorder = FALSE; | |
244 | ext_bit_reverse = FALSE; | |
245 | ext_level = 0; | |
246 | } | |
247 | ||
248 | void TTCN_Buffer::release_memory() | |
249 | { | |
250 | if (buf_ptr != NULL) { | |
251 | if (buf_ptr->ref_count > 1) buf_ptr->ref_count--; | |
252 | else if (buf_ptr->ref_count == 1) Free(buf_ptr); | |
253 | else { | |
254 | TTCN_EncDec_ErrorContext::error_internal("Invalid reference counter %u " | |
255 | "when freeing a TTCN_Buffer.", buf_ptr->ref_count); | |
256 | } | |
257 | } | |
258 | } | |
259 | ||
260 | size_t TTCN_Buffer::get_memory_size(size_t target_size) | |
261 | { | |
262 | size_t new_size = INITIAL_SIZE; | |
263 | while (new_size < target_size) { | |
264 | size_t next_size = new_size + new_size; | |
265 | if (next_size > new_size) new_size = next_size; | |
266 | else { | |
267 | // integer overflow occurred | |
268 | return static_cast<size_t>(-1); | |
269 | } | |
270 | } | |
271 | return new_size; | |
272 | } | |
273 | ||
274 | void TTCN_Buffer::copy_memory() | |
275 | { | |
276 | if (buf_ptr != NULL && buf_ptr->ref_count > 1) { | |
277 | buffer_struct *old_ptr = buf_ptr; | |
278 | old_ptr->ref_count--; | |
279 | buf_size = get_memory_size(buf_len); | |
280 | buf_ptr = (buffer_struct*)Malloc(MEMORY_SIZE(buf_size)); | |
281 | buf_ptr->ref_count = 1; | |
282 | memcpy(buf_ptr->data_ptr, old_ptr->data_ptr, buf_len); | |
283 | } | |
284 | } | |
285 | ||
286 | void TTCN_Buffer::increase_size(size_t size_incr) | |
287 | { | |
288 | if (buf_ptr != NULL) { | |
289 | size_t target_size = buf_len + size_incr; | |
290 | if (target_size < buf_len) | |
291 | TTCN_EncDec_ErrorContext::error_internal("TTCN_Buffer: Overflow error " | |
292 | "(cannot increase buffer size)."); // unsigned overflow | |
293 | if (buf_ptr->ref_count > 1) { // shared, need to split (copy-on-write) | |
294 | buffer_struct *old_ptr = buf_ptr; | |
295 | old_ptr->ref_count--; | |
296 | buf_size = get_memory_size(target_size); | |
297 | buf_ptr = (buffer_struct*)Malloc(MEMORY_SIZE(buf_size)); | |
298 | #ifndef NDEBUG | |
299 | memset(buf_ptr->data_ptr,0, buf_size); | |
300 | #endif | |
301 | buf_ptr->ref_count = 1; | |
302 | memcpy(buf_ptr->data_ptr, old_ptr->data_ptr, buf_len); | |
303 | } else if (target_size > buf_size) { // not shared, just change the size | |
304 | buf_size = get_memory_size(target_size); | |
305 | buf_ptr = (buffer_struct*)Realloc(buf_ptr, MEMORY_SIZE(buf_size)); | |
306 | #ifndef NDEBUG | |
307 | memset(buf_ptr->data_ptr + buf_len,0, buf_size - buf_len); | |
308 | #endif | |
309 | } | |
310 | } else { // a brand new buffer | |
311 | buf_size = get_memory_size(size_incr); | |
312 | buf_ptr = (buffer_struct*)Malloc(MEMORY_SIZE(buf_size)); | |
313 | #ifndef NDEBUG | |
314 | memset(buf_ptr->data_ptr,0, buf_size); | |
315 | #endif | |
316 | buf_ptr->ref_count = 1; | |
317 | } | |
318 | } | |
319 | ||
320 | TTCN_Buffer::TTCN_Buffer() | |
321 | { | |
322 | buf_ptr = NULL; | |
323 | buf_size = 0; | |
324 | buf_len = 0; | |
325 | reset_buffer(); | |
326 | } | |
327 | ||
328 | TTCN_Buffer::TTCN_Buffer(const TTCN_Buffer& p_buf) | |
329 | { | |
330 | buf_ptr = p_buf.buf_ptr; | |
331 | buf_ptr->ref_count++; | |
332 | buf_size = p_buf.buf_size; | |
333 | buf_len = p_buf.buf_len; | |
334 | reset_buffer(); | |
335 | } | |
336 | ||
337 | TTCN_Buffer::TTCN_Buffer(const OCTETSTRING& p_os) | |
338 | { | |
339 | p_os.must_bound("Initializing a TTCN_Buffer with an unbound octetstring " | |
340 | "value."); | |
341 | buf_ptr = (buffer_struct*)p_os.val_ptr; | |
342 | buf_ptr->ref_count++; | |
343 | buf_size = p_os.val_ptr->n_octets; | |
344 | buf_len = p_os.val_ptr->n_octets; | |
345 | reset_buffer(); | |
346 | } | |
347 | ||
348 | TTCN_Buffer::TTCN_Buffer(const CHARSTRING& p_cs) | |
349 | { | |
350 | p_cs.must_bound("Initializing a TTCN_Buffer with an unbound charstring " | |
351 | "value."); | |
352 | buf_ptr = (buffer_struct*)p_cs.val_ptr; | |
353 | buf_ptr->ref_count++; | |
354 | buf_size = p_cs.val_ptr->n_chars + 1; | |
355 | buf_len = p_cs.val_ptr->n_chars; | |
356 | reset_buffer(); | |
357 | } | |
358 | ||
359 | TTCN_Buffer& TTCN_Buffer::operator=(const TTCN_Buffer& p_buf) | |
360 | { | |
361 | if (&p_buf != this) { | |
362 | release_memory(); | |
363 | buf_ptr = p_buf.buf_ptr; | |
364 | buf_ptr->ref_count++; | |
365 | buf_size = p_buf.buf_size; | |
366 | buf_len = p_buf.buf_len; | |
367 | } | |
368 | reset_buffer(); | |
369 | return *this; | |
370 | } | |
371 | ||
372 | TTCN_Buffer& TTCN_Buffer::operator=(const OCTETSTRING& p_os) | |
373 | { | |
374 | p_os.must_bound("Assignment of an unbound octetstring value to a " | |
375 | "TTCN_Buffer."); | |
376 | release_memory(); | |
377 | buf_ptr = (buffer_struct*)p_os.val_ptr; | |
378 | buf_ptr->ref_count++; | |
379 | buf_size = p_os.val_ptr->n_octets; | |
380 | buf_len = p_os.val_ptr->n_octets; | |
381 | reset_buffer(); | |
382 | return *this; | |
383 | } | |
384 | ||
385 | TTCN_Buffer& TTCN_Buffer::operator=(const CHARSTRING& p_cs) | |
386 | { | |
387 | p_cs.must_bound("Assignment of an unbound charstring value to a " | |
388 | "TTCN_Buffer."); | |
389 | release_memory(); | |
390 | buf_ptr = (buffer_struct*)p_cs.val_ptr; | |
391 | buf_ptr->ref_count++; | |
392 | buf_size = p_cs.val_ptr->n_chars + 1; | |
393 | buf_len = p_cs.val_ptr->n_chars; | |
394 | reset_buffer(); | |
395 | return *this; | |
396 | } | |
397 | ||
398 | void TTCN_Buffer::clear() | |
399 | { | |
400 | release_memory(); | |
401 | buf_ptr = NULL; | |
402 | buf_size = 0; | |
403 | buf_len = 0; | |
404 | reset_buffer(); | |
405 | } | |
406 | ||
407 | const unsigned char* TTCN_Buffer::get_data() const | |
408 | { | |
409 | if (buf_ptr != NULL) return buf_ptr->data_ptr; | |
410 | else return NULL; | |
411 | } | |
412 | ||
413 | const unsigned char* TTCN_Buffer::get_read_data() const | |
414 | { | |
415 | if (buf_ptr != NULL) return buf_ptr->data_ptr + buf_pos; | |
416 | else return NULL; | |
417 | } | |
418 | ||
419 | void TTCN_Buffer::set_pos(size_t new_pos) | |
420 | { | |
421 | if (new_pos < buf_len) buf_pos = new_pos; | |
422 | else buf_pos = buf_len; | |
423 | } | |
424 | ||
425 | void TTCN_Buffer::increase_pos(size_t delta) | |
426 | { | |
427 | size_t new_buf_pos=buf_pos+delta; | |
428 | if(new_buf_pos<buf_pos || new_buf_pos>buf_len) | |
429 | buf_pos=buf_len; | |
430 | else | |
431 | buf_pos=new_buf_pos; | |
432 | } | |
433 | ||
434 | void TTCN_Buffer::get_end(unsigned char*& end_ptr, size_t& end_len) | |
435 | { | |
436 | increase_size(end_len); | |
437 | end_len = buf_size - buf_len; | |
438 | if (buf_ptr != NULL) end_ptr = buf_ptr->data_ptr + buf_len; | |
439 | else end_ptr = NULL; | |
440 | } | |
441 | ||
442 | void TTCN_Buffer::increase_length(size_t size_incr) | |
443 | { | |
444 | if (buf_size < buf_len + size_incr) increase_size(size_incr); | |
445 | buf_len += size_incr; | |
446 | } | |
447 | ||
448 | void TTCN_Buffer::put_c(unsigned char c) | |
449 | { | |
450 | increase_size(1); | |
451 | buf_ptr->data_ptr[buf_len] = c; | |
452 | buf_len++; | |
453 | } | |
454 | ||
455 | void TTCN_Buffer::put_s(size_t len, const unsigned char *s) | |
456 | { | |
457 | if (len > 0) { | |
458 | increase_size(len); | |
459 | memcpy(buf_ptr->data_ptr + buf_len, s, len); | |
460 | buf_len += len; | |
461 | } | |
462 | } | |
463 | ||
464 | void TTCN_Buffer::put_string(const OCTETSTRING& p_os) | |
465 | { | |
466 | p_os.must_bound("Appending an unbound octetstring value to a TTCN_Buffer."); | |
467 | if (p_os.val_ptr->n_octets > 0) { | |
468 | if (buf_len > 0) { | |
469 | increase_size(p_os.val_ptr->n_octets); | |
470 | memcpy(buf_ptr->data_ptr + buf_len, p_os.val_ptr->octets_ptr, | |
471 | p_os.val_ptr->n_octets); | |
472 | buf_len += p_os.val_ptr->n_octets; | |
473 | } else { | |
474 | release_memory(); | |
475 | buf_ptr = (buffer_struct*)p_os.val_ptr; | |
476 | buf_ptr->ref_count++; | |
477 | buf_size = p_os.val_ptr->n_octets; | |
478 | buf_len = p_os.val_ptr->n_octets; | |
479 | } | |
480 | } | |
481 | } | |
482 | ||
483 | void TTCN_Buffer::put_string(const CHARSTRING& p_cs) | |
484 | { | |
485 | p_cs.must_bound("Appending an unbound charstring value to a TTCN_Buffer."); | |
486 | if (p_cs.val_ptr->n_chars > 0) { // there is something in the CHARSTRING | |
487 | if (buf_len > 0) { // there is something in this buffer, append | |
488 | increase_size(p_cs.val_ptr->n_chars); | |
489 | memcpy(buf_ptr->data_ptr + buf_len, p_cs.val_ptr->chars_ptr, | |
490 | p_cs.val_ptr->n_chars); | |
491 | buf_len += p_cs.val_ptr->n_chars; | |
492 | } else { // share the data | |
493 | release_memory(); | |
494 | buf_ptr = (buffer_struct*)p_cs.val_ptr; | |
495 | buf_ptr->ref_count++; | |
496 | buf_size = p_cs.val_ptr->n_chars + 1; | |
497 | buf_len = p_cs.val_ptr->n_chars; | |
498 | } | |
499 | } | |
500 | } | |
501 | ||
502 | void TTCN_Buffer::put_buf(const TTCN_Buffer& p_buf) { | |
503 | if (p_buf.buf_ptr == 0) return; | |
504 | if (p_buf.buf_len > 0) { // there is something in the other buffer | |
505 | if (buf_len > 0) { // there is something in this buffer, append | |
506 | increase_size(p_buf.buf_len); | |
507 | memcpy(buf_ptr->data_ptr + buf_len, p_buf.buf_ptr->data_ptr, | |
508 | p_buf.buf_len); | |
509 | buf_len += p_buf.buf_len; | |
510 | } | |
511 | else { // share the data | |
512 | *this = p_buf; | |
513 | } | |
514 | } | |
515 | } | |
516 | ||
517 | void TTCN_Buffer::get_string(OCTETSTRING& p_os) | |
518 | { | |
519 | p_os.clean_up(); | |
520 | if (buf_len > 0) { | |
521 | if (buf_ptr->ref_count > 1) { | |
522 | p_os.init_struct(buf_len); | |
523 | memcpy(p_os.val_ptr->octets_ptr, buf_ptr->data_ptr, buf_len); | |
524 | } else { | |
525 | if (buf_size != buf_len) { | |
526 | buf_ptr = (buffer_struct*)Realloc(buf_ptr, MEMORY_SIZE(buf_len)); | |
527 | buf_size = buf_len; | |
528 | } | |
529 | p_os.val_ptr = (OCTETSTRING::octetstring_struct*)buf_ptr; | |
530 | p_os.val_ptr->ref_count++; | |
531 | p_os.val_ptr->n_octets = buf_len; | |
532 | } | |
533 | } else p_os.init_struct(0); | |
534 | } | |
535 | ||
536 | void TTCN_Buffer::get_string(CHARSTRING& p_cs) | |
537 | { | |
538 | p_cs.clean_up(); | |
539 | if (buf_len > 0) { | |
540 | if (buf_ptr->ref_count > 1) { // buffer is shared, copy is needed | |
541 | p_cs.init_struct(buf_len); | |
542 | memcpy(p_cs.val_ptr->chars_ptr, buf_ptr->data_ptr, buf_len); | |
543 | } else { // we are the sole owner | |
544 | // Share our buffer_struct with CHARSTRING's charstring_struct | |
545 | // (they have the same layout), after putting in a string terminator. | |
546 | if (buf_size != buf_len + 1) { | |
547 | buf_ptr = (buffer_struct*)Realloc(buf_ptr, MEMORY_SIZE(buf_len + 1)); | |
548 | buf_size = buf_len + 1; | |
549 | } | |
550 | p_cs.val_ptr = (CHARSTRING::charstring_struct*)buf_ptr; | |
551 | p_cs.val_ptr->ref_count++; | |
552 | p_cs.val_ptr->n_chars = buf_len; | |
553 | p_cs.val_ptr->chars_ptr[buf_len] = '\0'; | |
554 | } | |
555 | } else p_cs.init_struct(0); | |
556 | } | |
557 | ||
558 | void TTCN_Buffer::get_string(UNIVERSAL_CHARSTRING& p_cs) | |
559 | { | |
560 | p_cs.clean_up(); | |
561 | if (buf_len > 0) { | |
562 | // TODO what if not multiple of 4 ? | |
563 | p_cs.init_struct(buf_len / 4); | |
564 | memcpy(p_cs.val_ptr->uchars_ptr, buf_ptr->data_ptr, buf_len); | |
565 | } else p_cs.init_struct(0); | |
566 | } | |
567 | ||
568 | void TTCN_Buffer::cut() | |
569 | { | |
570 | if (buf_pos > 0) { | |
571 | if (buf_pos > buf_len) | |
572 | TTCN_EncDec_ErrorContext::error_internal("Read pointer points beyond " | |
573 | "the buffer end when cutting from a TTCN_Buffer."); | |
574 | size_t new_len = buf_len - buf_pos; | |
575 | if (new_len > 0) { | |
576 | if (buf_ptr->ref_count > 1) { | |
577 | buffer_struct *old_ptr = buf_ptr; | |
578 | old_ptr->ref_count--; | |
579 | buf_size = get_memory_size(new_len); | |
580 | buf_ptr = (buffer_struct*)Malloc(MEMORY_SIZE(buf_size)); | |
581 | buf_ptr->ref_count = 1; | |
582 | memcpy(buf_ptr->data_ptr, old_ptr->data_ptr + buf_pos, new_len); | |
583 | } else { | |
584 | memmove(buf_ptr->data_ptr, buf_ptr->data_ptr + buf_pos, new_len); | |
585 | size_t new_size = get_memory_size(new_len); | |
586 | if (new_size < buf_size) { | |
587 | buf_ptr = (buffer_struct*)Realloc(buf_ptr, MEMORY_SIZE(new_size)); | |
588 | buf_size = new_size; | |
589 | } | |
590 | } | |
591 | } else { | |
592 | release_memory(); | |
593 | buf_ptr = NULL; | |
594 | buf_size = 0; | |
595 | } | |
596 | buf_len = new_len; | |
597 | } | |
598 | reset_buffer(); | |
599 | } | |
600 | ||
601 | void TTCN_Buffer::cut_end() | |
602 | { | |
603 | if (buf_pos > buf_len) | |
604 | TTCN_EncDec_ErrorContext::error_internal("Read pointer points beyond " | |
605 | "the buffer end when cutting from a TTCN_Buffer."); | |
606 | if (buf_pos < buf_len) { | |
607 | if (buf_pos > 0) { | |
608 | if (buf_ptr == NULL) | |
609 | TTCN_EncDec_ErrorContext::error_internal("Data pointer is NULL when " | |
610 | "cutting from a TTCN_Buffer."); | |
611 | if (buf_ptr->ref_count == 1) { | |
612 | size_t new_size = get_memory_size(buf_pos); | |
613 | if (new_size < buf_size) { | |
614 | buf_ptr = (buffer_struct*)Realloc(buf_ptr, MEMORY_SIZE(new_size)); | |
615 | buf_size = new_size; | |
616 | } | |
617 | } | |
618 | } else { | |
619 | release_memory(); | |
620 | buf_ptr = NULL; | |
621 | buf_size = 0; | |
622 | } | |
623 | buf_len = buf_pos; | |
624 | } | |
625 | last_bit_pos = 0; | |
626 | last_bit_bitpos = 0; | |
627 | start_of_ext_bit = 0; | |
628 | last_bit = FALSE; | |
629 | current_bitorder = FALSE; | |
630 | ext_bit_reverse = FALSE; | |
631 | ext_level = 0; | |
632 | } | |
633 | ||
634 | boolean TTCN_Buffer::contains_complete_TLV() | |
635 | { | |
636 | if (buf_len > buf_pos) { | |
637 | ASN_BER_TLV_t tmp_tlv; | |
638 | return ASN_BER_str2TLV(buf_len - buf_pos, buf_ptr->data_ptr + buf_pos, | |
639 | tmp_tlv, BER_ACCEPT_ALL); | |
640 | } else return FALSE; | |
641 | } | |
642 | ||
643 | void TTCN_Buffer::log() const | |
644 | { | |
645 | TTCN_Logger::log_event("Buffer: size: %lu, pos: %lu, len: %lu data: (", | |
646 | (unsigned long)buf_size, (unsigned long)buf_pos, (unsigned long)buf_len); | |
647 | if (buf_len > 0) { | |
648 | const unsigned char *data_ptr = buf_ptr->data_ptr; | |
649 | for(size_t i=0; i<buf_pos; i++) | |
650 | TTCN_Logger::log_octet(data_ptr[i]); | |
651 | TTCN_Logger::log_event_str(" | "); | |
652 | for(size_t i=buf_pos; i<buf_len; i++) | |
653 | TTCN_Logger::log_octet(data_ptr[i]); | |
654 | } | |
655 | TTCN_Logger::log_char(')'); | |
656 | } | |
657 | ||
658 | void TTCN_Buffer::put_b(size_t len, const unsigned char *s, | |
659 | const RAW_coding_par& coding_par, int align) | |
660 | { | |
661 | ||
662 | unsigned char* st=NULL; | |
663 | unsigned char* st2=NULL; | |
664 | int loc_align=align<0?-align:align; | |
665 | bool must_align=false; | |
666 | raw_order_t local_bitorder=coding_par.bitorder; | |
667 | raw_order_t local_fieldorder=coding_par.fieldorder; | |
668 | if(current_bitorder) { | |
669 | if(local_bitorder==ORDER_LSB) local_bitorder=ORDER_MSB; | |
670 | else local_bitorder=ORDER_LSB; | |
671 | if(local_fieldorder==ORDER_LSB) local_fieldorder=ORDER_MSB; | |
672 | else local_fieldorder=ORDER_LSB; | |
673 | } | |
674 | /*printf("len:%d\r\n",len); | |
675 | printf("align:%d\r\n",align); | |
676 | printf("coding bito:%s,byte:%s,field:%s\r\n",coding_par.bitorder==ORDER_MSB?"M":"L", | |
677 | coding_par.byteorder==ORDER_MSB?"M":"L", | |
678 | coding_par.fieldorder==ORDER_MSB?"M":"L" | |
679 | ); | |
680 | printf("local bito:%s,field:%s\r\n",local_bitorder==ORDER_MSB?"M":"L", | |
681 | local_fieldorder==ORDER_MSB?"M":"L" | |
682 | );*/ | |
683 | if(align) { | |
684 | if((local_fieldorder==ORDER_LSB && (local_bitorder!=coding_par.byteorder))|| | |
685 | (local_fieldorder==ORDER_MSB && (local_bitorder==coding_par.byteorder))) { | |
686 | st=(unsigned char*)Malloc((len+loc_align+7)/8*sizeof(unsigned char)); | |
687 | memset(st,0,(len+loc_align+7)/8*sizeof(unsigned char)); | |
688 | if(align>0){ | |
689 | memcpy(st,s,(len+7)/8*sizeof(unsigned char)); | |
690 | if(len%8) st[(len+7)/8-1]&=BitMaskTable[len%8]; | |
691 | } | |
692 | else{ | |
693 | if(loc_align%8){ | |
694 | int bit_bound=loc_align%8; | |
695 | size_t max_index=(len+loc_align+7)/8-loc_align/8-1; | |
696 | unsigned char* ptr=st+loc_align/8; | |
697 | unsigned char mask=BitMaskTable[bit_bound]; | |
698 | for(size_t a=0;a<(len+7)/8;a++){ | |
699 | ptr[a]&=mask; | |
700 | ptr[a]|=s[a]<<(8-bit_bound); | |
701 | if(a<max_index) ptr[a+1]=s[a]>>bit_bound; | |
702 | } | |
703 | } | |
704 | else{ | |
705 | memcpy(st+loc_align/8,s,(len+7)/8*sizeof(unsigned char)); | |
706 | } | |
707 | } | |
708 | s=st; | |
709 | len+=loc_align; | |
710 | } | |
711 | else{ | |
712 | if(coding_par.byteorder==ORDER_MSB) align=-align; | |
713 | if(align<0) put_zero(loc_align,local_fieldorder); | |
714 | else must_align=true; | |
715 | } | |
716 | } | |
717 | if(len==0) { | |
718 | if(must_align) put_zero(loc_align,local_fieldorder); | |
719 | return; | |
720 | } | |
721 | size_t new_size=((bit_pos==0?buf_len*8:buf_len*8-(8-bit_pos))+len+7)/8; | |
722 | size_t new_bit_pos=(bit_pos+len)%8; | |
723 | if (new_size > buf_len) increase_size(new_size - buf_len); | |
724 | else copy_memory(); | |
725 | unsigned char *data_ptr = buf_ptr != NULL ? buf_ptr->data_ptr : NULL; | |
726 | //printf("buf_len:%d bit_pos:%d\r\n",buf_len,bit_pos); | |
727 | //printf("new_size:%d new_bit_pos:%d\r\n",new_size,new_bit_pos); | |
728 | if(coding_par.hexorder==ORDER_MSB){ | |
729 | st2=(unsigned char*)Malloc((len+7)/8*sizeof(unsigned char)); | |
730 | if(bit_pos==4){ | |
731 | st2[0]=s[0]; | |
732 | for(size_t a=1;a<(len+7)/8;a++){ | |
733 | unsigned char ch='\0'; | |
734 | ch|=s[a-1]>>4; | |
735 | st2[a-1]=(st2[a-1]&0x0f)|(s[a]<<4); | |
736 | st2[a]=(s[a]&0xf0)|ch; | |
737 | } | |
738 | } | |
739 | else{ | |
740 | for(size_t a=0;a<(len+7)/8;a++) st2[a]=(s[a]<<4)|(s[a]>>4); | |
741 | if(len%8) st2[(len+7)/8]>>=4; | |
742 | } | |
743 | s=st2; | |
744 | } | |
745 | if(bit_pos+len<=8){ // there is enough space within 1 octet to store the data | |
746 | if(local_bitorder==ORDER_LSB){ | |
747 | if(local_fieldorder==ORDER_LSB){ | |
748 | data_ptr[new_size-1]= | |
749 | (data_ptr[new_size-1]&BitMaskTable[bit_pos])| | |
750 | (s[0]<<bit_pos); | |
751 | }else{ | |
752 | data_ptr[new_size-1]= | |
753 | (data_ptr[new_size-1]&~BitMaskTable[8-bit_pos])| | |
754 | ((s[0]&BitMaskTable[len])<<(8-bit_pos-len)); | |
755 | } | |
756 | } | |
757 | else{ | |
758 | if(local_fieldorder==ORDER_LSB){ | |
759 | data_ptr[new_size-1]= | |
760 | (data_ptr[new_size-1]&BitMaskTable[bit_pos])| | |
761 | (REVERSE_BITS(s[0])>>(8-len-bit_pos)); | |
762 | }else{ | |
763 | data_ptr[new_size-1]= | |
764 | (data_ptr[new_size-1]&~BitMaskTable[8-bit_pos])| | |
765 | (REVERSE_BITS(s[0]&BitMaskTable[len])>>bit_pos); | |
766 | } | |
767 | } | |
768 | } | |
769 | else if(bit_pos==0 && (len%8)==0){ // octet aligned data | |
770 | if(coding_par.byteorder==ORDER_LSB){ | |
771 | if(local_bitorder==ORDER_LSB){ | |
772 | memcpy(data_ptr+buf_len, s, len/8); | |
773 | } | |
774 | else{ | |
775 | unsigned char *prt=data_ptr+buf_len; | |
776 | for(size_t a=0;a<len/8;a++) prt[a]=REVERSE_BITS(s[a]); | |
777 | } | |
778 | } | |
779 | else{ | |
780 | if(local_bitorder==ORDER_LSB){ | |
781 | unsigned char *prt=data_ptr+buf_len; | |
782 | for(size_t a=0,b=len/8-1;a<len/8;a++,b--) prt[a]=s[b]; | |
783 | } | |
784 | else{ | |
785 | unsigned char *prt=data_ptr+buf_len; | |
786 | for(size_t a=0,b=len/8-1;a<len/8;a++,b--) prt[a]=REVERSE_BITS(s[b]); | |
787 | } | |
788 | } | |
789 | } | |
790 | else{ | |
791 | size_t maxindex=new_size-1; | |
792 | if(coding_par.byteorder==ORDER_LSB){ | |
793 | if(local_bitorder==ORDER_LSB){ | |
794 | if(bit_pos){ | |
795 | unsigned char mask1=BitMaskTable[bit_pos]; | |
796 | unsigned char *prt=data_ptr+(buf_len==0?0:buf_len-1); | |
797 | if(local_fieldorder==ORDER_MSB){ | |
798 | unsigned int num_bytes = (len+7) / 8; | |
799 | unsigned int active_bits_in_last = len % 8; | |
3abe9331 | 800 | if(!active_bits_in_last) active_bits_in_last=8; |
970ed795 EL |
801 | for(unsigned int a=0; a < num_bytes; a++){ |
802 | prt[a]&=REVERSE_BITS(mask1); | |
803 | unsigned char sa = s[a]; | |
804 | if (a == num_bytes - 1) { // last byte | |
805 | sa <<= (8 - active_bits_in_last); | |
806 | // push bits up so the first active bit is in MSB | |
807 | } | |
808 | prt[a]|=(sa>>bit_pos)&~REVERSE_BITS(mask1); | |
809 | if(a<maxindex) | |
810 | prt[a+1]=sa<<(8-bit_pos); | |
811 | } | |
812 | } | |
813 | else{ | |
814 | for(unsigned int a=0;a<(len+7)/8;a++){ | |
815 | prt[a]&=mask1; | |
816 | prt[a]|=s[a]<<bit_pos; | |
817 | if(a<maxindex) | |
818 | prt[a+1]=s[a]>>(8-bit_pos); | |
819 | } | |
820 | } | |
821 | } | |
822 | else{ // start from octet boundary | |
823 | memcpy(data_ptr+buf_len, s, (len+7)/8*sizeof(unsigned char)); | |
3abe9331 | 824 | if(local_fieldorder==ORDER_MSB && new_bit_pos){ |
825 | data_ptr[new_size-1]<<=(8-new_bit_pos); | |
826 | ||
827 | } | |
970ed795 EL |
828 | } |
829 | } | |
830 | else{ // bitorder==ORDER_MSB | |
831 | if(bit_pos){ | |
832 | unsigned char mask1=REVERSE_BITS(BitMaskTable[bit_pos]); | |
833 | unsigned char *prt=data_ptr+(buf_len==0?0:buf_len-1); | |
834 | if(local_fieldorder==ORDER_MSB){ | |
835 | prt[0]&=mask1; | |
836 | prt[0]|=REVERSE_BITS(s[0])>>bit_pos; | |
837 | prt[1]=REVERSE_BITS(s[0])<<(8-bit_pos); | |
838 | } | |
839 | else{ | |
840 | prt[0]&=REVERSE_BITS(mask1); | |
841 | prt[0]|=REVERSE_BITS(s[0])&~REVERSE_BITS(mask1); | |
842 | prt[1]=REVERSE_BITS(s[0])<<(8-bit_pos); | |
843 | } | |
844 | for(unsigned int a=1;a<(len+7)/8;a++){ | |
845 | prt[a]&=mask1; | |
846 | prt[a]|=REVERSE_BITS(s[a])>>bit_pos; | |
847 | if(a<maxindex) | |
848 | prt[a+1]=REVERSE_BITS(s[a])<<(8-bit_pos); | |
849 | } | |
850 | } | |
851 | else{ // start from octet boundary | |
852 | unsigned char *prt=data_ptr+buf_len; | |
853 | for(unsigned int a=0;a<(len+7)/8;a++) prt[a]=REVERSE_BITS(s[a]); | |
854 | } | |
855 | if(local_fieldorder==ORDER_LSB && new_bit_pos) | |
856 | data_ptr[new_size-1]>>=(8-new_bit_pos); | |
857 | } | |
858 | } | |
859 | else{ // byteorder==ORDER_MSB | |
860 | if(local_bitorder==ORDER_LSB){ | |
861 | if(bit_pos){ | |
862 | unsigned char mask1=BitMaskTable[bit_pos]; | |
863 | unsigned char ch=get_byte_rev(s,len,0); | |
864 | unsigned char *prt=data_ptr+(buf_len==0?0:buf_len-1); | |
865 | if(local_fieldorder==ORDER_MSB){ | |
866 | prt[0]&=REVERSE_BITS(mask1); | |
867 | prt[0]|=ch>>bit_pos; | |
868 | prt[1]=ch<<(8-bit_pos); | |
869 | } | |
870 | else{ | |
871 | prt[0]&=mask1; | |
872 | prt[0]|=ch&~mask1; | |
873 | prt[1]=ch<<(8-bit_pos); | |
874 | } | |
875 | for(unsigned int a=1;a<(len+7)/8;a++){ | |
876 | ch=get_byte_rev(s,len,a); | |
877 | prt[a]&=REVERSE_BITS(mask1); | |
878 | prt[a]|=ch>>bit_pos; | |
879 | if(a<maxindex) | |
880 | prt[a+1]=ch<<(8-bit_pos); | |
881 | } | |
882 | } | |
883 | else{ | |
884 | unsigned char *prt=data_ptr+buf_len; | |
885 | for(unsigned int a=0;a<(len+7)/8;a++) prt[a]=get_byte_rev(s,len,a); | |
886 | } | |
887 | if(local_fieldorder==ORDER_LSB && new_bit_pos) | |
888 | data_ptr[new_size-1]>>=(8-new_bit_pos); | |
889 | } | |
890 | else{ // bitorder==ORDER_MSB | |
891 | if(bit_pos){ | |
892 | unsigned char mask1=BitMaskTable[bit_pos]; | |
893 | unsigned char ch=get_byte_rev(s,len,0); | |
894 | unsigned char *prt=data_ptr+(buf_len==0?0:buf_len-1); | |
895 | if(local_fieldorder==ORDER_MSB){ | |
896 | prt[0]&=REVERSE_BITS(mask1); | |
897 | prt[0]|=REVERSE_BITS(ch)&~REVERSE_BITS(mask1); | |
898 | prt[1]=REVERSE_BITS(ch)>>(8-bit_pos); | |
899 | } | |
900 | else{ | |
901 | prt[0]&=mask1; | |
902 | prt[0]|=REVERSE_BITS(ch)<<bit_pos; | |
903 | prt[1]=REVERSE_BITS(ch)>>(8-bit_pos); | |
904 | } | |
905 | for(unsigned int a=1;a<(len+7)/8;a++){ | |
906 | ch=get_byte_rev(s,len,a); | |
907 | prt[a]&=mask1; | |
908 | prt[a]|=REVERSE_BITS(ch)<<bit_pos; | |
909 | if(a<maxindex) | |
910 | prt[a+1]=REVERSE_BITS(ch)>>(8-bit_pos); | |
911 | } | |
912 | } | |
913 | else{ // start from octet boundary | |
914 | unsigned char *prt=data_ptr+buf_len; | |
915 | for(unsigned int a=0;a<(len+7)/8;a++) prt[a]= | |
916 | REVERSE_BITS(get_byte_rev(s,len,a)); | |
917 | } | |
918 | if(local_fieldorder==ORDER_MSB && new_bit_pos) | |
919 | data_ptr[new_size-1]<<=(8-new_bit_pos); | |
920 | } | |
921 | } | |
922 | } | |
923 | if(st) Free(st); | |
924 | if(st2) Free(st2); | |
925 | /* last_bit_pos=((bit_pos==0?buf_len*8:buf_len*8-(8-bit_pos))+len+6)/8; | |
926 | if(local_fieldorder==ORDER_LSB) | |
927 | last_bit_bitpos=(bit_pos+len-1)%8; | |
928 | else | |
929 | last_bit_bitpos=7-(bit_pos+len-1)%8;*/ | |
930 | buf_len=new_size; | |
931 | bit_pos=new_bit_pos; | |
932 | if(bit_pos){ | |
933 | last_bit_pos=buf_len-1; | |
934 | if(local_fieldorder==ORDER_LSB) | |
935 | last_bit_bitpos=bit_pos-1; | |
936 | else | |
937 | last_bit_bitpos=7-(bit_pos-1); | |
938 | } | |
939 | else{ | |
940 | last_bit_pos=buf_len-1; | |
941 | if(local_fieldorder==ORDER_LSB) | |
942 | last_bit_bitpos=7; | |
943 | else | |
944 | last_bit_bitpos=0; | |
945 | } | |
946 | if(must_align) put_zero(loc_align,local_fieldorder); | |
947 | } | |
948 | ||
949 | void TTCN_Buffer::get_b(size_t len, unsigned char *s, | |
950 | const RAW_coding_par& coding_par, raw_order_t top_bit_order) | |
951 | { | |
952 | if(len==0) return; | |
953 | size_t new_buf_pos=buf_pos+(bit_pos+len)/8; | |
954 | size_t new_bit_pos=(bit_pos+len)%8; | |
955 | raw_order_t local_bitorder=coding_par.bitorder; | |
956 | raw_order_t local_fieldorder=coding_par.fieldorder; | |
957 | if(top_bit_order==ORDER_LSB){ | |
958 | if(local_bitorder==ORDER_LSB) local_bitorder=ORDER_MSB; | |
959 | else local_bitorder=ORDER_LSB; | |
960 | if(local_fieldorder==ORDER_LSB) local_fieldorder=ORDER_MSB; | |
961 | else local_fieldorder=ORDER_LSB; | |
962 | } | |
963 | const unsigned char *data_ptr = buf_ptr != NULL ? buf_ptr->data_ptr : NULL; | |
964 | if(bit_pos+len<=8){ // the data is within 1 octet | |
965 | if(local_bitorder==ORDER_LSB){ | |
966 | if(local_fieldorder==ORDER_LSB){ | |
967 | s[0]=data_ptr[buf_pos]>>bit_pos; | |
968 | }else{ | |
969 | s[0]=data_ptr[buf_pos]>>(8-bit_pos-len); | |
970 | } | |
971 | } | |
972 | else{ | |
973 | if(local_fieldorder==ORDER_LSB){ | |
974 | s[0]=REVERSE_BITS(data_ptr[buf_pos])>>(8-bit_pos-len); | |
975 | }else{ | |
976 | s[0]=REVERSE_BITS(data_ptr[buf_pos])>>bit_pos; | |
977 | } | |
978 | } | |
979 | } | |
980 | else if(bit_pos==0 && (len%8)==0){ // octet aligned data | |
981 | if(coding_par.byteorder==ORDER_LSB){ | |
982 | if(local_bitorder==ORDER_LSB){ | |
983 | memcpy(s, data_ptr+buf_pos, len/8*sizeof(unsigned char)); | |
984 | } | |
985 | else{ | |
986 | const unsigned char *prt=data_ptr+buf_pos; | |
987 | for(size_t a=0;a<len/8;a++) s[a]=REVERSE_BITS(prt[a]); | |
988 | } | |
989 | } | |
990 | else{ | |
991 | if(local_bitorder==ORDER_LSB){ | |
992 | const unsigned char *prt=data_ptr+buf_pos; | |
993 | for(size_t a=0,b=len/8-1;a<len/8;a++,b--) s[a]=prt[b]; | |
994 | } | |
995 | else{ | |
996 | const unsigned char *prt=data_ptr+buf_pos; | |
997 | for(size_t a=0,b=len/8-1;a<len/8;a++,b--) s[a]=REVERSE_BITS(prt[b]); | |
998 | } | |
999 | } | |
1000 | } | |
1001 | else{ // unaligned | |
1002 | size_t num_bytes = (len + 7) / 8; | |
1003 | if(coding_par.byteorder==ORDER_LSB){ | |
1004 | if(local_bitorder==ORDER_LSB){ | |
1005 | if(bit_pos){ | |
1006 | unsigned char mask1=BitMaskTable[8-bit_pos]; | |
1007 | if(local_fieldorder==ORDER_LSB){ | |
1008 | for(unsigned int a=0;a<num_bytes;a++){ | |
1009 | s[a]=(get_byte_align(len,local_fieldorder,ORDER_MSB,a+1) | |
1010 | <<(8-bit_pos))| | |
1011 | ((get_byte_align(len,local_fieldorder,ORDER_MSB,a) | |
1012 | >>bit_pos)&mask1); | |
1013 | } | |
1014 | } else { | |
1015 | mask1=BitMaskTable[bit_pos]; | |
1016 | for(unsigned int a=0;a<num_bytes;a++){ | |
1017 | s[a]=(get_byte_align(len,local_fieldorder,ORDER_LSB,a+1) | |
1018 | >>(8-bit_pos)&mask1)| | |
1019 | ((get_byte_align(len,local_fieldorder,ORDER_LSB,a) | |
1020 | <<bit_pos)); | |
1021 | } | |
1022 | int active_bits_in_last_byte = len % 8; | |
1023 | if (active_bits_in_last_byte) { | |
1024 | s[num_bytes - 1] >>= (8 - active_bits_in_last_byte); | |
1025 | } | |
1026 | } | |
1027 | } | |
1028 | else{ // start from octet boundary | |
1029 | memcpy(s, data_ptr+buf_pos, num_bytes*sizeof(unsigned char)); | |
1030 | if(local_fieldorder==ORDER_MSB && new_bit_pos) | |
1031 | s[num_bytes-1]>>=(8-new_bit_pos); | |
1032 | } | |
1033 | } | |
1034 | else{ // bitorder==ORDER_MSB | |
1035 | if(bit_pos){ | |
1036 | unsigned char mask1=BitMaskTable[bit_pos]; | |
1037 | for(unsigned int a=0;a<num_bytes;a++){ | |
1038 | s[a]=REVERSE_BITS( | |
1039 | ((get_byte_align(len,local_fieldorder,ORDER_LSB,a+1) | |
1040 | >>(8-bit_pos))&mask1)| | |
1041 | (get_byte_align(len,local_fieldorder,ORDER_LSB,a) | |
1042 | <<bit_pos)); | |
1043 | } | |
1044 | } | |
1045 | else{ // start from octet boundary | |
1046 | const unsigned char *prt=data_ptr+buf_pos; | |
1047 | for(unsigned int a=0;a<num_bytes;a++) s[a]=REVERSE_BITS(prt[a]); | |
1048 | if(local_fieldorder==ORDER_LSB && new_bit_pos) | |
1049 | s[num_bytes-1]>>=(8-new_bit_pos); | |
1050 | } | |
1051 | } | |
1052 | } | |
1053 | else{ // byteorder==ORDER_MSB | |
1054 | if(local_bitorder==ORDER_LSB){ | |
1055 | if(new_bit_pos){ | |
1056 | unsigned char mask1=BitMaskTable[new_bit_pos]; | |
1057 | for(unsigned int a=0,b=(bit_pos+len)/8;a<num_bytes;a++,b--){ | |
1058 | s[a]=((get_byte_align(len,local_fieldorder,ORDER_LSB,b) | |
1059 | >>(8-new_bit_pos))&mask1)| | |
1060 | (get_byte_align(len,local_fieldorder,ORDER_LSB,b-1) | |
1061 | <<new_bit_pos); | |
1062 | } | |
1063 | } | |
1064 | else{ | |
1065 | // unsigned char *prt=data_ptr+buf_pos; | |
1066 | for(unsigned int a=0,b=new_buf_pos-1;a<num_bytes;a++,b--) | |
1067 | s[a]=data_ptr[b]; | |
1068 | if(local_fieldorder==ORDER_LSB && bit_pos) | |
1069 | s[num_bytes-1]>>=bit_pos; | |
1070 | } | |
1071 | } | |
1072 | else{ // bitorder==ORDER_MSB | |
1073 | if(new_bit_pos){ | |
1074 | // unsigned char mask1=BitMaskTable[new_bit_pos]; | |
1075 | for(unsigned int a=0,b=(bit_pos+len)/8;a<num_bytes;a++,b--){ | |
1076 | s[a]=REVERSE_BITS( | |
1077 | (get_byte_align(len,local_fieldorder,ORDER_MSB,b) | |
1078 | <<(8-new_bit_pos))| | |
1079 | (get_byte_align(len,local_fieldorder,ORDER_MSB,b-1) | |
1080 | >>new_bit_pos)); | |
1081 | } | |
1082 | } | |
1083 | else{ // start from octet boundary | |
1084 | // unsigned char *prt=data_ptr+buf_pos; | |
1085 | for(unsigned int a=0,b=new_buf_pos-1;a<num_bytes;a++,b--) | |
1086 | s[a]=REVERSE_BITS(data_ptr[b]); | |
1087 | if(local_fieldorder==ORDER_MSB && bit_pos) | |
1088 | s[num_bytes-1]>>=bit_pos; | |
1089 | } | |
1090 | } | |
1091 | } | |
1092 | } | |
1093 | if(coding_par.hexorder==ORDER_MSB){ | |
1094 | if(bit_pos==4){ | |
1095 | for(size_t a=1;a<(len+7)/8;a++){ | |
1096 | unsigned char ch='\0'; | |
1097 | ch|=s[a-1]>>4; | |
1098 | s[a-1]=(s[a-1]&0x0f)|(s[a]<<4); | |
1099 | s[a]=(s[a]&0xf0)|ch; | |
1100 | } | |
1101 | } | |
1102 | else{ | |
1103 | for(size_t a=0;a<(len+7)/8;a++) s[a]=(s[a]<<4)|(s[a]>>4); | |
1104 | if(len%8) s[(len+7)/8]>>=4; | |
1105 | } | |
1106 | } | |
1107 | ||
1108 | size_t last_bit_offset = bit_pos + len - 1; | |
1109 | unsigned char last_bit_octet = data_ptr[buf_pos + last_bit_offset / 8]; | |
1110 | if (local_fieldorder == ORDER_LSB) last_bit_octet >>= last_bit_offset % 8; | |
1111 | else last_bit_octet >>= 7 - last_bit_offset % 8; | |
1112 | if (last_bit_octet & 0x01) last_bit = TRUE; | |
1113 | else last_bit = FALSE; | |
1114 | ||
1115 | buf_pos=new_buf_pos; | |
1116 | bit_pos=new_bit_pos; | |
1117 | } | |
1118 | ||
1119 | void TTCN_Buffer::put_zero(size_t len, raw_order_t fieldorder) | |
1120 | { | |
1121 | if(len==0) return; | |
1122 | size_t new_size=((bit_pos==0?buf_len*8:buf_len*8-(8-bit_pos))+len+7)/8; | |
1123 | if (new_size > buf_len) increase_size(new_size - buf_len); | |
1124 | else copy_memory(); | |
1125 | unsigned char *data_ptr = buf_ptr != NULL ? buf_ptr->data_ptr : NULL; | |
1126 | if(bit_pos){ | |
1127 | if(bit_pos+len>8){ | |
1128 | unsigned char mask1=BitMaskTable[bit_pos]; | |
1129 | unsigned char *prt=data_ptr+(buf_len==0?0:buf_len-1); | |
1130 | if(fieldorder==ORDER_LSB) prt[0]&=mask1; | |
1131 | else prt[0]&=~mask1; | |
1132 | memset(prt+1, 0, (len-1+bit_pos)/8); | |
1133 | } | |
1134 | else { | |
1135 | if(fieldorder==ORDER_LSB) | |
1136 | data_ptr[new_size-1]=data_ptr[new_size-1]&BitMaskTable[bit_pos]; | |
1137 | else | |
1138 | data_ptr[new_size-1]=data_ptr[new_size-1]& | |
1139 | REVERSE_BITS(BitMaskTable[bit_pos]); | |
1140 | } | |
1141 | } | |
1142 | else { | |
1143 | memset(data_ptr+buf_len, 0, (len+7)/8); | |
1144 | } | |
1145 | buf_len=new_size; | |
1146 | bit_pos=(bit_pos+len)%8; | |
1147 | if(bit_pos){ | |
1148 | last_bit_pos=buf_len-1; | |
1149 | if(fieldorder==ORDER_LSB) | |
1150 | last_bit_bitpos=bit_pos-1; | |
1151 | else | |
1152 | last_bit_bitpos=7-(bit_pos-1); | |
1153 | } | |
1154 | else{ | |
1155 | last_bit_pos=buf_len-1; | |
1156 | if(fieldorder==ORDER_LSB) | |
1157 | last_bit_bitpos=7; | |
1158 | else | |
1159 | last_bit_bitpos=0; | |
1160 | } | |
1161 | } | |
1162 | ||
1163 | const unsigned char* TTCN_Buffer::get_read_data(size_t &bitpos) const | |
1164 | { | |
1165 | bitpos=bit_pos; | |
1166 | if (buf_ptr != NULL) return buf_ptr->data_ptr + buf_pos; | |
1167 | else return NULL; | |
1168 | } | |
1169 | ||
1170 | void TTCN_Buffer::set_pos(size_t pos, size_t bitpos) | |
1171 | { | |
1172 | buf_pos=pos<buf_len?pos:buf_len; | |
1173 | bit_pos=bitpos; | |
1174 | } | |
1175 | ||
1176 | ||
1177 | void TTCN_Buffer::set_pos_bit(size_t new_bit_pos) | |
1178 | { | |
1179 | size_t new_pos = new_bit_pos / 8; | |
1180 | if (new_pos < buf_len) { | |
1181 | buf_pos = new_pos; | |
1182 | bit_pos = new_bit_pos % 8; | |
1183 | } else { | |
1184 | buf_pos = buf_len; | |
1185 | bit_pos = 0; | |
1186 | } | |
1187 | } | |
1188 | ||
1189 | void TTCN_Buffer::increase_pos_bit(size_t delta) | |
1190 | { | |
1191 | size_t new_buf_pos=buf_pos+(bit_pos+delta)/8; // bytes | |
1192 | if(new_buf_pos<buf_pos || new_buf_pos>buf_len) { | |
1193 | buf_pos=buf_len; | |
1194 | bit_pos=7; | |
1195 | } | |
1196 | else { | |
1197 | buf_pos=new_buf_pos; | |
1198 | bit_pos=(bit_pos+delta)%8; | |
1199 | } | |
1200 | } | |
1201 | ||
1202 | int TTCN_Buffer::increase_pos_padd(int padding) | |
1203 | { | |
1204 | if(padding) { // <---old bit pos---> | |
1205 | size_t new_bit_pos = ((buf_pos*8 + bit_pos + padding-1)/padding) * padding; | |
1206 | int padded = new_bit_pos - buf_pos * 8 - bit_pos; | |
1207 | // padded = bits skipped to reach the next multiple of padding (bits) | |
1208 | buf_pos = new_bit_pos / 8; | |
1209 | bit_pos = new_bit_pos % 8; | |
1210 | return padded; | |
1211 | }else | |
1212 | return 0; | |
1213 | } | |
1214 | ||
1215 | size_t TTCN_Buffer::unread_len_bit() | |
1216 | { | |
1217 | return (buf_len-buf_pos)*8-bit_pos; | |
1218 | } | |
1219 | ||
1220 | void TTCN_Buffer::start_ext_bit(boolean p_reverse) | |
1221 | { | |
1222 | if (ext_level++ == 0) { | |
1223 | start_of_ext_bit = buf_len; | |
1224 | ext_bit_reverse = p_reverse; | |
1225 | } | |
1226 | } | |
1227 | ||
1228 | void TTCN_Buffer::stop_ext_bit() | |
1229 | { | |
1230 | if (ext_level <= 0) | |
1231 | TTCN_EncDec_ErrorContext::error_internal("TTCN_Buffer::stop_ext_bit() " | |
1232 | "was called without start_ext_bit()."); | |
1233 | if (--ext_level == 0) { | |
1234 | unsigned char one = current_bitorder ? 0x01 : 0x80; | |
1235 | unsigned char zero= ~one; | |
1236 | unsigned char *data_ptr = buf_ptr != NULL ? buf_ptr->data_ptr : NULL; | |
1237 | if (ext_bit_reverse) { | |
1238 | for(size_t a=start_of_ext_bit;a<buf_len-1;a++) data_ptr[a] |= one; | |
1239 | data_ptr[buf_len-1] &= zero; | |
1240 | } else { | |
1241 | for(size_t a=start_of_ext_bit;a<buf_len-1;a++) data_ptr[a] &= zero; | |
1242 | data_ptr[buf_len-1] |= one; | |
1243 | } | |
1244 | } | |
1245 | } | |
1246 | ||
1247 | void TTCN_Buffer::put_pad(size_t len, const unsigned char *s, | |
1248 | int pat_len, raw_order_t fieldorder) | |
1249 | { | |
1250 | if(len==0) return; | |
1251 | if(pat_len==0){ | |
1252 | put_zero(len,fieldorder); | |
1253 | return; | |
1254 | } | |
1255 | RAW_coding_par cp; | |
1256 | cp.bitorder=ORDER_LSB; | |
1257 | cp.byteorder=ORDER_LSB; | |
1258 | cp.fieldorder=fieldorder; | |
1259 | cp.hexorder=ORDER_LSB; | |
1260 | int length=len; | |
1261 | while(length>0){ | |
1262 | put_b(length>pat_len?pat_len:length,s,cp,0); | |
1263 | length-=pat_len; | |
1264 | } | |
1265 | } | |
1266 | ||
1267 | void TTCN_Buffer::set_last_bit(boolean p_last_bit) | |
1268 | { | |
1269 | unsigned char *last_bit_ptr = buf_ptr->data_ptr + last_bit_pos; | |
1270 | unsigned char bitmask = 0x01 << last_bit_bitpos; | |
1271 | if (p_last_bit) *last_bit_ptr |= bitmask; | |
1272 | else *last_bit_ptr &= ~bitmask; | |
1273 | } | |
1274 | ||
1275 | unsigned char TTCN_Buffer::get_byte_rev(const unsigned char* data, | |
1276 | size_t len, size_t idx) | |
1277 | { | |
1278 | unsigned char ch='\0'; | |
1279 | size_t hossz=(len+7)/8-1; | |
1280 | int bit_limit=len%8; | |
1281 | if(idx>hossz) return ch; | |
1282 | if(bit_limit==0)return data[hossz-idx]; | |
1283 | ch=data[hossz-idx]<<(8-bit_limit); | |
1284 | if((hossz-idx)>0) ch|=(data[hossz-idx-1]>>bit_limit) | |
1285 | &BitMaskTable[8-bit_limit]; | |
1286 | return ch; | |
1287 | } | |
1288 | ||
1289 | unsigned char TTCN_Buffer::get_byte_align(size_t len, | |
1290 | raw_order_t fieldorder, | |
1291 | raw_order_t req_align, | |
1292 | size_t idx) | |
1293 | { | |
1294 | if(idx>(bit_pos+len)/8) return '\0'; | |
1295 | const unsigned char *data_ptr = buf_ptr != NULL ? buf_ptr->data_ptr : NULL; | |
1296 | if(idx==0){ // first byte | |
1297 | if(fieldorder==req_align){ | |
1298 | if(fieldorder==ORDER_LSB){ | |
1299 | return data_ptr[buf_pos]>>bit_pos; | |
1300 | } | |
1301 | else {return data_ptr[buf_pos]<<bit_pos;} | |
1302 | } | |
1303 | else {return data_ptr[buf_pos];} | |
1304 | } | |
1305 | if(idx==(bit_pos+len)/8){ // last byte | |
1306 | if(fieldorder==req_align){ | |
1307 | if(fieldorder==ORDER_LSB){ | |
1308 | return data_ptr[buf_pos+idx]<<(8-(bit_pos+len)%8); | |
1309 | } | |
1310 | else {return data_ptr[buf_pos+idx]>>(8-(bit_pos+len)%8);} | |
1311 | } | |
1312 | else {return data_ptr[buf_pos+idx];} | |
1313 | } | |
1314 | return data_ptr[buf_pos+idx]; | |
1315 | } |