]> git.stg.codes - stg.git/blob - libs/smux/per_encoder.c
Use std::lock_guard instead of STG_LOCKER.
[stg.git] / libs / smux / per_encoder.c
1 #include <asn_application.h>
2 #include <asn_internal.h>
3 #include <per_encoder.h>
4
5 static int _uper_encode_flush_outp(asn_per_outp_t *po);
6
7 static int
8 ignore_output(const void *data, size_t size, void *app_key) {
9     (void)data;
10     (void)size;
11     (void)app_key;
12     return 0;
13 }
14
15 asn_enc_rval_t
16 uper_encode(const asn_TYPE_descriptor_t *td,
17             const asn_per_constraints_t *constraints, const void *sptr,
18             asn_app_consume_bytes_f *cb, void *app_key) {
19     asn_per_outp_t po;
20     asn_enc_rval_t er;
21
22     /*
23      * Invoke type-specific encoder.
24      */
25     if(!td || !td->op->uper_encoder)
26         ASN__ENCODE_FAILED;     /* PER is not compiled in */
27
28     po.buffer = po.tmpspace;
29     po.nboff = 0;
30     po.nbits = 8 * sizeof(po.tmpspace);
31     po.output = cb ? cb : ignore_output;
32     po.op_key = app_key;
33     po.flushed_bytes = 0;
34
35     er = td->op->uper_encoder(td, constraints, sptr, &po);
36     if(er.encoded != -1) {
37         size_t bits_to_flush;
38
39         bits_to_flush = ((po.buffer - po.tmpspace) << 3) + po.nboff;
40
41         /* Set number of bits encoded to a firm value */
42         er.encoded = (po.flushed_bytes << 3) + bits_to_flush;
43
44         if(_uper_encode_flush_outp(&po)) ASN__ENCODE_FAILED;
45     }
46
47     return er;
48 }
49
50 /*
51  * Argument type and callback necessary for uper_encode_to_buffer().
52  */
53 typedef struct enc_to_buf_arg {
54         void *buffer;
55         size_t left;
56 } enc_to_buf_arg;
57 static int encode_to_buffer_cb(const void *buffer, size_t size, void *key) {
58         enc_to_buf_arg *arg = (enc_to_buf_arg *)key;
59
60         if(arg->left < size)
61                 return -1;      /* Data exceeds the available buffer size */
62
63         memcpy(arg->buffer, buffer, size);
64         arg->buffer = ((char *)arg->buffer) + size;
65         arg->left -= size;
66
67         return 0;
68 }
69
70 asn_enc_rval_t
71 uper_encode_to_buffer(const asn_TYPE_descriptor_t *td,
72                       const asn_per_constraints_t *constraints,
73                       const void *sptr, void *buffer, size_t buffer_size) {
74     enc_to_buf_arg key;
75
76     key.buffer = buffer;
77     key.left = buffer_size;
78
79     if(td) ASN_DEBUG("Encoding \"%s\" using UNALIGNED PER", td->name);
80
81     return uper_encode(td, constraints, sptr, encode_to_buffer_cb, &key);
82 }
83
84 typedef struct enc_dyn_arg {
85         void *buffer;
86         size_t length;
87         size_t allocated;
88 } enc_dyn_arg;
89 static int
90 encode_dyn_cb(const void *buffer, size_t size, void *key) {
91     enc_dyn_arg *arg = key;
92     if(arg->length + size >= arg->allocated) {
93         size_t new_size = arg->allocated ? arg->allocated : 8;
94         void *p;
95
96         do {
97             new_size <<= 2;
98         } while(arg->length + size >= new_size);
99
100         p = REALLOC(arg->buffer, new_size);
101         if(!p) {
102             FREEMEM(arg->buffer);
103             memset(arg, 0, sizeof(*arg));
104             return -1;
105         }
106         arg->buffer = p;
107         arg->allocated = new_size;
108     }
109     memcpy(((char *)arg->buffer) + arg->length, buffer, size);
110     arg->length += size;
111     return 0;
112 }
113 ssize_t
114 uper_encode_to_new_buffer(const asn_TYPE_descriptor_t *td,
115                           const asn_per_constraints_t *constraints,
116                           const void *sptr, void **buffer_r) {
117     asn_enc_rval_t er;
118         enc_dyn_arg key;
119
120         memset(&key, 0, sizeof(key));
121
122         er = uper_encode(td, constraints, sptr, encode_dyn_cb, &key);
123         switch(er.encoded) {
124         case -1:
125                 FREEMEM(key.buffer);
126                 return -1;
127         case 0:
128                 FREEMEM(key.buffer);
129                 key.buffer = MALLOC(1);
130                 if(key.buffer) {
131                         *(char *)key.buffer = '\0';
132                         *buffer_r = key.buffer;
133                         return 1;
134                 } else {
135                         return -1;
136                 }
137         default:
138                 *buffer_r = key.buffer;
139                 ASN_DEBUG("Complete encoded in %ld bits", (long)er.encoded);
140                 return ((er.encoded + 7) >> 3);
141         }
142 }
143
144 /*
145  * Internally useful functions.
146  */
147
148 /* Flush partially filled buffer */
149 static int
150 _uper_encode_flush_outp(asn_per_outp_t *po) {
151         uint8_t *buf;
152
153         if(po->nboff == 0 && po->buffer == po->tmpspace)
154                 return 0;
155
156         buf = po->buffer + (po->nboff >> 3);
157         /* Make sure we account for the last, partially filled */
158         if(po->nboff & 0x07) {
159                 buf[0] &= 0xff << (8 - (po->nboff & 0x07));
160                 buf++;
161         }
162
163         return po->output(po->tmpspace, buf - po->tmpspace, po->op_key);
164 }
165