1: #include "global.h"
2:
3:
4: #pragma GCC visibility push(hidden)
5:
6: int mqtt_Errno;
7: char mqtt_Error[STRSIZ];
8:
9: #pragma GCC visibility pop
10:
11: //
12: // Error maintenance functions ...
13: //
14:
15: // mqtt_GetErrno() Get error code of last operation
16: inline int
17: mqtt_GetErrno()
18: {
19: return mqtt_Errno;
20: }
21:
22: // mqtt_GetError() Get error text of last operation
23: inline const char *
24: mqtt_GetError()
25: {
26: return mqtt_Error;
27: }
28:
29: // mqtt_SetErr() Set error to variables for internal use!!!
30: inline void
31: mqtt_SetErr(int eno, char *estr, ...)
32: {
33: va_list lst;
34:
35: mqtt_Errno = eno;
36: memset(mqtt_Error, 0, sizeof mqtt_Error);
37: va_start(lst, estr);
38: vsnprintf(mqtt_Error, sizeof mqtt_Error, estr, lst);
39: va_end(lst);
40: }
41:
42: #pragma GCC visibility push(hidden)
43: // _mqtt_readHEADER() read fixed header from MQTT message
44: inline struct mqtthdr *
45: _mqtt_readHEADER(mqtt_msg_t * __restrict buf, u_char cmd, int *bytes, int *len)
46: {
47: struct mqtthdr *hdr;
48:
49: if (!buf || !buf->msg_base || !buf->msg_len)
50: return NULL;
51:
52: hdr = (struct mqtthdr*) buf->msg_base;
53: if (hdr->mqtt_msg.type != cmd) {
54: mqtt_SetErr(EINVAL, "Error:: wrong command #%d should be %d",
55: hdr->mqtt_msg.type, cmd);
56: return NULL;
57: }
58:
59: *len = mqtt_decodeLen(hdr->mqtt_len, bytes);
60: return hdr;
61: }
62: #pragma GCC visibility pop
63:
64: // ----------------------------------------------------------
65:
66: /*
67: * mqtt_msgFree() Free MQTT message
68: *
69: * @msg = Message buffer
70: * @all = !=0 Destroy entire message, if MQTT Message allocated with mqtt_msgAlloc()
71: * return: none
72: */
73: inline void
74: mqtt_msgFree(mqtt_msg_t ** __restrict msg, int all)
75: {
76: if (msg && *msg) {
77: if ((*msg)->msg_base) {
78: free((*msg)->msg_base);
79: (*msg)->msg_base = NULL;
80: }
81: if (all) {
82: free(*msg);
83: *msg = NULL;
84: } else
85: (*msg)->msg_len ^= (*msg)->msg_len;
86: }
87: }
88:
89: /*
90: * mqtt_msgAlloc() Allocate memory for MQTT Message
91: *
92: * @len = >0 Allocate buffer with length
93: * return: NULL error or Message, after use must call mqtt_msgFree() with all!=0
94: */
95: inline mqtt_msg_t *
96: mqtt_msgAlloc(u_short len)
97: {
98: mqtt_msg_t *m = NULL;
99:
100: m = malloc(sizeof(mqtt_msg_t));
101: if (!m) {
102: LOGERR;
103: return NULL;
104: } else
105: memset(m, 0, sizeof(mqtt_msg_t));
106:
107: if (len) {
108: m->msg_len = len;
109: m->msg_base = malloc(m->msg_len);
110: if (!m->msg_base) {
111: LOGERR;
112: free(m);
113: return NULL;
114: } else
115: memset(m->msg_base, 0, m->msg_len);
116: }
117:
118: return m;
119: }
120:
121: /*
122: * mqtt_msgRealloc() Reallocate MQTT message buffer
123: *
124: * @msg = MQTT message
125: * @len = new length
126: * return: -1 error or >-1 old buffer length
127: */
128: inline int
129: mqtt_msgRealloc(mqtt_msg_t * __restrict msg, u_short len)
130: {
131: void *p = NULL;
132: int ret = 0;
133:
134: if (!msg)
135: return -1;
136:
137: if (len == msg->msg_len)
138: return len;
139:
140: p = realloc(msg->msg_base, len);
141: if (!p) {
142: LOGERR;
143: return -1;
144: }
145:
146: ret = msg->msg_len;
147: msg->msg_len = len;
148: msg->msg_base = p;
149:
150: return ret;
151: }
152:
153: /*
154: * mqtt_encodeLen() Encode number to MQTT length field
155: *
156: * @num = number for encode
157: * return: -1 error or >-1 length
158: */
159: inline u_int
160: mqtt_encodeLen(u_int num)
161: {
162: register u_int dig, i;
163: u_int ret = 0;
164:
165: if (num > 268435455)
166: return (u_int) -1;
167:
168: for (i = 0; i < sizeof ret && num > 0; i++) {
169: dig = num % 0x80;
170: num /= 0x80;
171: if (num > 0)
172: dig |= 0x80;
173:
174: *((u_char*) &ret + i) = (u_char) dig;
175: }
176:
177: return ret;
178: }
179:
180: /*
181: * mqtt_decodeLen() Decode length from MQTT packet
182: *
183: * @len = length from MQTT header
184: * @n = sizeof bytes, if !=NULL
185: * return: -1 error, >-1 length of message
186: */
187: inline u_int
188: mqtt_decodeLen(void * __restrict len, int * __restrict n)
189: {
190: register u_int i, dig, mul;
191: u_int ret = 0;
192: u_char *p = (u_char*) len;
193:
194: if (!len)
195: return (u_int) -1;
196:
197: for (mul = 1, i = 0; i < sizeof ret; i++, mul *= 0x80) {
198: dig = p[i];
199: ret += (dig & 0x7f) * mul;
200:
201: if (!(dig & 0x80))
202: break;
203: }
204:
205: if (n)
206: *n = (char) (i & 0x7f) + 1;
207: return ret;
208: }
209:
210: /*
211: * mqtt_sizeLen Return sizeof len field
212: *
213: * @len = length
214: * return: -1 error, >-1 sizeof len in bytes
215: */
216: inline char
217: mqtt_sizeLen(u_int len)
218: {
219: register char i;
220: u_char *p = (u_char*) &len;
221:
222: if (len > 0xffffff7f)
223: return -1;
224:
225: for (i = 0; i < sizeof len; i++)
226: if (!(*(p + i) & 0x80))
227: break;
228:
229: return ++i;
230: }
231:
232: /*
233: * mqtt_str2sub Create MQTT subscribe variable from string(s)
234: *
235: * @csStr = strings
236: * @strnum = number of strings elements
237: * @qoses = QoS elements applied to subscribe variable,
238: * count of elements must be equal with csStr elements
239: * return: NULL error or != subscribe variables array, must be free after use with mqtt_freeSub()
240: */
241: inline mqtt_subscr_t *
242: mqtt_str2sub(const char **csStr, u_short strnum, u_char *qoses)
243: {
244: mqtt_subscr_t *v;
245: register int i, items;
246: const char **strs;
247:
248: if (!csStr)
249: return NULL;
250: for (items = 0, strs = csStr; *strs; items++, strs++)
251: if (strnum && items >= strnum) {
252: items = strnum;
253: break;
254: }
255:
256: if (!(v = malloc((items + 1) * sizeof(mqtt_subscr_t)))) {
257: LOGERR;
258: return NULL;
259: } else
260: memset(v, 0, (items + 1) * sizeof(mqtt_subscr_t));
261:
262: for (i = 0; i < items; i++) {
263: v[i].sub_topic._size = strlen(csStr[i]);
264: v[i].sub_topic._base = (u_char*) strdup(csStr[i]);
265: if (qoses && qoses[i] < MQTT_QOS_RESERVED)
266: v[i].sub_ret = qoses[i];
267: }
268:
269: return v;
270: }
271:
272: /*
273: * mqtt_subFree() Free array from subscribe variables
274: *
275: * @subs = Subscribe variables
276: * return: none
277: */
278: inline void
279: mqtt_subFree(mqtt_subscr_t ** __restrict subs)
280: {
281: mqtt_subscr_t *v;
282:
283: if (!subs)
284: return;
285:
286: for (v = *subs; v->sub_topic._base; v++) {
287: free(v->sub_topic._base);
288: v->sub_topic._base = NULL;
289: v->sub_topic._size = 0;
290:
291: if (v->sub_value._base) {
292: free(v->sub_value._base);
293: v->sub_value._base = NULL;
294: v->sub_value._size = 0;
295: }
296: }
297:
298: free(*subs);
299: *subs = NULL;
300: }
301:
302: /*
303: * mqtt_subAlloc() Create array from subscribe variables
304: *
305: * @num = Number of elements
306: * return: NULL error or subscribe array, after use must call mqtt_subFree()
307: */
308: inline mqtt_subscr_t *
309: mqtt_subAlloc(u_short num)
310: {
311: mqtt_subscr_t *s = NULL;
312:
313: s = malloc((num + 1) * sizeof(mqtt_subscr_t));
314: if (!s) {
315: LOGERR;
316: return NULL;
317: } else
318: memset(s, 0, (num + 1) * sizeof(mqtt_subscr_t));
319:
320: return s;
321: }
322:
323: /*
324: * mqtt_subRealloc() Reallocate array from subscribe variables
325: *
326: * @subs = Subscribe array
327: * @num = Number of elements
328: * return: NULL error or subscribe array, after use must call mqtt_subFree()
329: */
330: inline mqtt_subscr_t *
331: mqtt_subRealloc(mqtt_subscr_t * __restrict subs, u_short num)
332: {
333: mqtt_subscr_t *s = NULL;
334:
335: s = realloc(subs, (num + 1) * sizeof(mqtt_subscr_t));
336: if (!s) {
337: LOGERR;
338: return NULL;
339: }
340:
341: return s;
342: }
FreeBSD-CVSweb <freebsd-cvsweb@FreeBSD.org>
![[BACK]](/icons/cvsweb/back.gif){kind=icon}
Return to aitmqtt.c CVS log ![[TXT]](/icons/cvsweb/text.gif){kind=icon}
![[DIR]](/icons/cvsweb/dir.gif){kind=icon}
Up to [ELWIX - Embedded LightWeight unIX -] / mqtt / src