1 | /* $Id: capiutil.c,v 1.13.6.4 2001/09/23 22:24:33 kai Exp $ |
2 | * |
3 | * CAPI 2.0 convert capi message to capi message struct |
4 | * |
5 | * From CAPI 2.0 Development Kit AVM 1995 (msg.c) |
6 | * Rewritten for Linux 1996 by Carsten Paeth <calle@calle.de> |
7 | * |
8 | * This software may be used and distributed according to the terms |
9 | * of the GNU General Public License, incorporated herein by reference. |
10 | * |
11 | */ |
12 | |
13 | #include <linux/module.h> |
14 | #include <linux/string.h> |
15 | #include <linux/ctype.h> |
16 | #include <linux/stddef.h> |
17 | #include <linux/kernel.h> |
18 | #include <linux/mm.h> |
19 | #include <linux/init.h> |
20 | #include <linux/isdn/capiutil.h> |
21 | #include <linux/slab.h> |
22 | |
23 | #include "kcapi.h" |
24 | |
25 | /* from CAPI2.0 DDK AVM Berlin GmbH */ |
26 | |
27 | typedef struct { |
28 | int typ; |
29 | size_t off; |
30 | } _cdef; |
31 | |
32 | #define _CBYTE 1 |
33 | #define _CWORD 2 |
34 | #define _CDWORD 3 |
35 | #define _CSTRUCT 4 |
36 | #define _CMSTRUCT 5 |
37 | #define _CEND 6 |
38 | |
39 | static _cdef cdef[] = |
40 | { |
41 | /*00 */ |
42 | {_CEND}, |
43 | /*01 */ |
44 | {_CEND}, |
45 | /*02 */ |
46 | {_CEND}, |
47 | /*03 */ |
48 | {_CDWORD, offsetof(_cmsg, adr.adrController)}, |
49 | /*04 */ |
50 | {_CMSTRUCT, offsetof(_cmsg, AdditionalInfo)}, |
51 | /*05 */ |
52 | {_CSTRUCT, offsetof(_cmsg, B1configuration)}, |
53 | /*06 */ |
54 | {_CWORD, offsetof(_cmsg, B1protocol)}, |
55 | /*07 */ |
56 | {_CSTRUCT, offsetof(_cmsg, B2configuration)}, |
57 | /*08 */ |
58 | {_CWORD, offsetof(_cmsg, B2protocol)}, |
59 | /*09 */ |
60 | {_CSTRUCT, offsetof(_cmsg, B3configuration)}, |
61 | /*0a */ |
62 | {_CWORD, offsetof(_cmsg, B3protocol)}, |
63 | /*0b */ |
64 | {_CSTRUCT, offsetof(_cmsg, BC)}, |
65 | /*0c */ |
66 | {_CSTRUCT, offsetof(_cmsg, BChannelinformation)}, |
67 | /*0d */ |
68 | {_CMSTRUCT, offsetof(_cmsg, BProtocol)}, |
69 | /*0e */ |
70 | {_CSTRUCT, offsetof(_cmsg, CalledPartyNumber)}, |
71 | /*0f */ |
72 | {_CSTRUCT, offsetof(_cmsg, CalledPartySubaddress)}, |
73 | /*10 */ |
74 | {_CSTRUCT, offsetof(_cmsg, CallingPartyNumber)}, |
75 | /*11 */ |
76 | {_CSTRUCT, offsetof(_cmsg, CallingPartySubaddress)}, |
77 | /*12 */ |
78 | {_CDWORD, offsetof(_cmsg, CIPmask)}, |
79 | /*13 */ |
80 | {_CDWORD, offsetof(_cmsg, CIPmask2)}, |
81 | /*14 */ |
82 | {_CWORD, offsetof(_cmsg, CIPValue)}, |
83 | /*15 */ |
84 | {_CDWORD, offsetof(_cmsg, Class)}, |
85 | /*16 */ |
86 | {_CSTRUCT, offsetof(_cmsg, ConnectedNumber)}, |
87 | /*17 */ |
88 | {_CSTRUCT, offsetof(_cmsg, ConnectedSubaddress)}, |
89 | /*18 */ |
90 | {_CDWORD, offsetof(_cmsg, Data)}, |
91 | /*19 */ |
92 | {_CWORD, offsetof(_cmsg, DataHandle)}, |
93 | /*1a */ |
94 | {_CWORD, offsetof(_cmsg, DataLength)}, |
95 | /*1b */ |
96 | {_CSTRUCT, offsetof(_cmsg, FacilityConfirmationParameter)}, |
97 | /*1c */ |
98 | {_CSTRUCT, offsetof(_cmsg, Facilitydataarray)}, |
99 | /*1d */ |
100 | {_CSTRUCT, offsetof(_cmsg, FacilityIndicationParameter)}, |
101 | /*1e */ |
102 | {_CSTRUCT, offsetof(_cmsg, FacilityRequestParameter)}, |
103 | /*1f */ |
104 | {_CWORD, offsetof(_cmsg, FacilitySelector)}, |
105 | /*20 */ |
106 | {_CWORD, offsetof(_cmsg, Flags)}, |
107 | /*21 */ |
108 | {_CDWORD, offsetof(_cmsg, Function)}, |
109 | /*22 */ |
110 | {_CSTRUCT, offsetof(_cmsg, HLC)}, |
111 | /*23 */ |
112 | {_CWORD, offsetof(_cmsg, Info)}, |
113 | /*24 */ |
114 | {_CSTRUCT, offsetof(_cmsg, InfoElement)}, |
115 | /*25 */ |
116 | {_CDWORD, offsetof(_cmsg, InfoMask)}, |
117 | /*26 */ |
118 | {_CWORD, offsetof(_cmsg, InfoNumber)}, |
119 | /*27 */ |
120 | {_CSTRUCT, offsetof(_cmsg, Keypadfacility)}, |
121 | /*28 */ |
122 | {_CSTRUCT, offsetof(_cmsg, LLC)}, |
123 | /*29 */ |
124 | {_CSTRUCT, offsetof(_cmsg, ManuData)}, |
125 | /*2a */ |
126 | {_CDWORD, offsetof(_cmsg, ManuID)}, |
127 | /*2b */ |
128 | {_CSTRUCT, offsetof(_cmsg, NCPI)}, |
129 | /*2c */ |
130 | {_CWORD, offsetof(_cmsg, Reason)}, |
131 | /*2d */ |
132 | {_CWORD, offsetof(_cmsg, Reason_B3)}, |
133 | /*2e */ |
134 | {_CWORD, offsetof(_cmsg, Reject)}, |
135 | /*2f */ |
136 | {_CSTRUCT, offsetof(_cmsg, Useruserdata)} |
137 | }; |
138 | |
139 | static unsigned char *cpars[] = |
140 | { |
141 | /* ALERT_REQ */ [0x01] = "\x03\x04\x0c\x27\x2f\x1c\x01\x01" , |
142 | /* CONNECT_REQ */ [0x02] = "\x03\x14\x0e\x10\x0f\x11\x0d\x06\x08\x0a\x05\x07\x09\x01\x0b\x28\x22\x04\x0c\x27\x2f\x1c\x01\x01" , |
143 | /* DISCONNECT_REQ */ [0x04] = "\x03\x04\x0c\x27\x2f\x1c\x01\x01" , |
144 | /* LISTEN_REQ */ [0x05] = "\x03\x25\x12\x13\x10\x11\x01" , |
145 | /* INFO_REQ */ [0x08] = "\x03\x0e\x04\x0c\x27\x2f\x1c\x01\x01" , |
146 | /* FACILITY_REQ */ [0x09] = "\x03\x1f\x1e\x01" , |
147 | /* SELECT_B_PROTOCOL_REQ */ [0x0a] = "\x03\x0d\x06\x08\x0a\x05\x07\x09\x01\x01" , |
148 | /* CONNECT_B3_REQ */ [0x0b] = "\x03\x2b\x01" , |
149 | /* DISCONNECT_B3_REQ */ [0x0d] = "\x03\x2b\x01" , |
150 | /* DATA_B3_REQ */ [0x0f] = "\x03\x18\x1a\x19\x20\x01" , |
151 | /* RESET_B3_REQ */ [0x10] = "\x03\x2b\x01" , |
152 | /* ALERT_CONF */ [0x13] = "\x03\x23\x01" , |
153 | /* CONNECT_CONF */ [0x14] = "\x03\x23\x01" , |
154 | /* DISCONNECT_CONF */ [0x16] = "\x03\x23\x01" , |
155 | /* LISTEN_CONF */ [0x17] = "\x03\x23\x01" , |
156 | /* MANUFACTURER_REQ */ [0x18] = "\x03\x2a\x15\x21\x29\x01" , |
157 | /* INFO_CONF */ [0x1a] = "\x03\x23\x01" , |
158 | /* FACILITY_CONF */ [0x1b] = "\x03\x23\x1f\x1b\x01" , |
159 | /* SELECT_B_PROTOCOL_CONF */ [0x1c] = "\x03\x23\x01" , |
160 | /* CONNECT_B3_CONF */ [0x1d] = "\x03\x23\x01" , |
161 | /* DISCONNECT_B3_CONF */ [0x1f] = "\x03\x23\x01" , |
162 | /* DATA_B3_CONF */ [0x21] = "\x03\x19\x23\x01" , |
163 | /* RESET_B3_CONF */ [0x22] = "\x03\x23\x01" , |
164 | /* CONNECT_IND */ [0x26] = "\x03\x14\x0e\x10\x0f\x11\x0b\x28\x22\x04\x0c\x27\x2f\x1c\x01\x01" , |
165 | /* CONNECT_ACTIVE_IND */ [0x27] = "\x03\x16\x17\x28\x01" , |
166 | /* DISCONNECT_IND */ [0x28] = "\x03\x2c\x01" , |
167 | /* MANUFACTURER_CONF */ [0x2a] = "\x03\x2a\x15\x21\x29\x01" , |
168 | /* INFO_IND */ [0x2c] = "\x03\x26\x24\x01" , |
169 | /* FACILITY_IND */ [0x2d] = "\x03\x1f\x1d\x01" , |
170 | /* CONNECT_B3_IND */ [0x2f] = "\x03\x2b\x01" , |
171 | /* CONNECT_B3_ACTIVE_IND */ [0x30] = "\x03\x2b\x01" , |
172 | /* DISCONNECT_B3_IND */ [0x31] = "\x03\x2d\x2b\x01" , |
173 | /* DATA_B3_IND */ [0x33] = "\x03\x18\x1a\x19\x20\x01" , |
174 | /* RESET_B3_IND */ [0x34] = "\x03\x2b\x01" , |
175 | /* CONNECT_B3_T90_ACTIVE_IND */ [0x35] = "\x03\x2b\x01" , |
176 | /* CONNECT_RESP */ [0x38] = "\x03\x2e\x0d\x06\x08\x0a\x05\x07\x09\x01\x16\x17\x28\x04\x0c\x27\x2f\x1c\x01\x01" , |
177 | /* CONNECT_ACTIVE_RESP */ [0x39] = "\x03\x01" , |
178 | /* DISCONNECT_RESP */ [0x3a] = "\x03\x01" , |
179 | /* MANUFACTURER_IND */ [0x3c] = "\x03\x2a\x15\x21\x29\x01" , |
180 | /* INFO_RESP */ [0x3e] = "\x03\x01" , |
181 | /* FACILITY_RESP */ [0x3f] = "\x03\x1f\x01" , |
182 | /* CONNECT_B3_RESP */ [0x41] = "\x03\x2e\x2b\x01" , |
183 | /* CONNECT_B3_ACTIVE_RESP */ [0x42] = "\x03\x01" , |
184 | /* DISCONNECT_B3_RESP */ [0x43] = "\x03\x01" , |
185 | /* DATA_B3_RESP */ [0x45] = "\x03\x19\x01" , |
186 | /* RESET_B3_RESP */ [0x46] = "\x03\x01" , |
187 | /* CONNECT_B3_T90_ACTIVE_RESP */ [0x47] = "\x03\x01" , |
188 | /* MANUFACTURER_RESP */ [0x4e] = "\x03\x2a\x15\x21\x29\x01" , |
189 | }; |
190 | |
191 | /*-------------------------------------------------------*/ |
192 | |
193 | #define byteTLcpy(x, y) *(u8 *)(x) = *(u8 *)(y); |
194 | #define wordTLcpy(x, y) *(u16 *)(x) = *(u16 *)(y); |
195 | #define dwordTLcpy(x, y) memcpy(x, y, 4); |
196 | #define structTLcpy(x, y, l) memcpy(x, y, l) |
197 | #define structTLcpyovl(x, y, l) memmove(x, y, l) |
198 | |
199 | #define byteTRcpy(x, y) *(u8 *)(y) = *(u8 *)(x); |
200 | #define wordTRcpy(x, y) *(u16 *)(y) = *(u16 *)(x); |
201 | #define dwordTRcpy(x, y) memcpy(y, x, 4); |
202 | #define structTRcpy(x, y, l) memcpy(y, x, l) |
203 | #define structTRcpyovl(x, y, l) memmove(y, x, l) |
204 | |
205 | /*-------------------------------------------------------*/ |
206 | static unsigned command_2_index(u8 c, u8 sc) |
207 | { |
208 | if (c & 0x80) |
209 | c = 0x9 + (c & 0x0f); |
210 | else if (c == 0x41) |
211 | c = 0x9 + 0x1; |
212 | if (c > 0x18) |
213 | c = 0x00; |
214 | return (sc & 3) * (0x9 + 0x9) + c; |
215 | } |
216 | |
217 | /** |
218 | * capi_cmd2par() - find parameter string for CAPI 2.0 command/subcommand |
219 | * @cmd: command number |
220 | * @subcmd: subcommand number |
221 | * |
222 | * Return value: static string, NULL if command/subcommand unknown |
223 | */ |
224 | |
225 | static unsigned char *capi_cmd2par(u8 cmd, u8 subcmd) |
226 | { |
227 | return cpars[command_2_index(c: cmd, sc: subcmd)]; |
228 | } |
229 | |
230 | /*-------------------------------------------------------*/ |
231 | #define TYP (cdef[cmsg->par[cmsg->p]].typ) |
232 | #define OFF (((u8 *)cmsg) + cdef[cmsg->par[cmsg->p]].off) |
233 | |
234 | static void jumpcstruct(_cmsg *cmsg) |
235 | { |
236 | unsigned layer; |
237 | for (cmsg->p++, layer = 1; layer;) { |
238 | /* $$$$$ assert (cmsg->p); */ |
239 | cmsg->p++; |
240 | switch (TYP) { |
241 | case _CMSTRUCT: |
242 | layer++; |
243 | break; |
244 | case _CEND: |
245 | layer--; |
246 | break; |
247 | } |
248 | } |
249 | } |
250 | |
251 | /*-------------------------------------------------------*/ |
252 | |
253 | static char *mnames[] = |
254 | { |
255 | [0x01] = "ALERT_REQ" , |
256 | [0x02] = "CONNECT_REQ" , |
257 | [0x04] = "DISCONNECT_REQ" , |
258 | [0x05] = "LISTEN_REQ" , |
259 | [0x08] = "INFO_REQ" , |
260 | [0x09] = "FACILITY_REQ" , |
261 | [0x0a] = "SELECT_B_PROTOCOL_REQ" , |
262 | [0x0b] = "CONNECT_B3_REQ" , |
263 | [0x0d] = "DISCONNECT_B3_REQ" , |
264 | [0x0f] = "DATA_B3_REQ" , |
265 | [0x10] = "RESET_B3_REQ" , |
266 | [0x13] = "ALERT_CONF" , |
267 | [0x14] = "CONNECT_CONF" , |
268 | [0x16] = "DISCONNECT_CONF" , |
269 | [0x17] = "LISTEN_CONF" , |
270 | [0x18] = "MANUFACTURER_REQ" , |
271 | [0x1a] = "INFO_CONF" , |
272 | [0x1b] = "FACILITY_CONF" , |
273 | [0x1c] = "SELECT_B_PROTOCOL_CONF" , |
274 | [0x1d] = "CONNECT_B3_CONF" , |
275 | [0x1f] = "DISCONNECT_B3_CONF" , |
276 | [0x21] = "DATA_B3_CONF" , |
277 | [0x22] = "RESET_B3_CONF" , |
278 | [0x26] = "CONNECT_IND" , |
279 | [0x27] = "CONNECT_ACTIVE_IND" , |
280 | [0x28] = "DISCONNECT_IND" , |
281 | [0x2a] = "MANUFACTURER_CONF" , |
282 | [0x2c] = "INFO_IND" , |
283 | [0x2d] = "FACILITY_IND" , |
284 | [0x2f] = "CONNECT_B3_IND" , |
285 | [0x30] = "CONNECT_B3_ACTIVE_IND" , |
286 | [0x31] = "DISCONNECT_B3_IND" , |
287 | [0x33] = "DATA_B3_IND" , |
288 | [0x34] = "RESET_B3_IND" , |
289 | [0x35] = "CONNECT_B3_T90_ACTIVE_IND" , |
290 | [0x38] = "CONNECT_RESP" , |
291 | [0x39] = "CONNECT_ACTIVE_RESP" , |
292 | [0x3a] = "DISCONNECT_RESP" , |
293 | [0x3c] = "MANUFACTURER_IND" , |
294 | [0x3e] = "INFO_RESP" , |
295 | [0x3f] = "FACILITY_RESP" , |
296 | [0x41] = "CONNECT_B3_RESP" , |
297 | [0x42] = "CONNECT_B3_ACTIVE_RESP" , |
298 | [0x43] = "DISCONNECT_B3_RESP" , |
299 | [0x45] = "DATA_B3_RESP" , |
300 | [0x46] = "RESET_B3_RESP" , |
301 | [0x47] = "CONNECT_B3_T90_ACTIVE_RESP" , |
302 | [0x4e] = "MANUFACTURER_RESP" |
303 | }; |
304 | |
305 | /** |
306 | * capi_cmd2str() - convert CAPI 2.0 command/subcommand number to name |
307 | * @cmd: command number |
308 | * @subcmd: subcommand number |
309 | * |
310 | * Return value: static string |
311 | */ |
312 | |
313 | char *capi_cmd2str(u8 cmd, u8 subcmd) |
314 | { |
315 | char *result; |
316 | |
317 | result = mnames[command_2_index(c: cmd, sc: subcmd)]; |
318 | if (result == NULL) |
319 | result = "INVALID_COMMAND" ; |
320 | return result; |
321 | } |
322 | |
323 | |
324 | /*-------------------------------------------------------*/ |
325 | |
326 | #ifdef CONFIG_CAPI_TRACE |
327 | |
328 | /*-------------------------------------------------------*/ |
329 | |
330 | static char *pnames[] = |
331 | { |
332 | /*00 */ NULL, |
333 | /*01 */ NULL, |
334 | /*02 */ NULL, |
335 | /*03 */ "Controller/PLCI/NCCI" , |
336 | /*04 */ "AdditionalInfo" , |
337 | /*05 */ "B1configuration" , |
338 | /*06 */ "B1protocol" , |
339 | /*07 */ "B2configuration" , |
340 | /*08 */ "B2protocol" , |
341 | /*09 */ "B3configuration" , |
342 | /*0a */ "B3protocol" , |
343 | /*0b */ "BC" , |
344 | /*0c */ "BChannelinformation" , |
345 | /*0d */ "BProtocol" , |
346 | /*0e */ "CalledPartyNumber" , |
347 | /*0f */ "CalledPartySubaddress" , |
348 | /*10 */ "CallingPartyNumber" , |
349 | /*11 */ "CallingPartySubaddress" , |
350 | /*12 */ "CIPmask" , |
351 | /*13 */ "CIPmask2" , |
352 | /*14 */ "CIPValue" , |
353 | /*15 */ "Class" , |
354 | /*16 */ "ConnectedNumber" , |
355 | /*17 */ "ConnectedSubaddress" , |
356 | /*18 */ "Data32" , |
357 | /*19 */ "DataHandle" , |
358 | /*1a */ "DataLength" , |
359 | /*1b */ "FacilityConfirmationParameter" , |
360 | /*1c */ "Facilitydataarray" , |
361 | /*1d */ "FacilityIndicationParameter" , |
362 | /*1e */ "FacilityRequestParameter" , |
363 | /*1f */ "FacilitySelector" , |
364 | /*20 */ "Flags" , |
365 | /*21 */ "Function" , |
366 | /*22 */ "HLC" , |
367 | /*23 */ "Info" , |
368 | /*24 */ "InfoElement" , |
369 | /*25 */ "InfoMask" , |
370 | /*26 */ "InfoNumber" , |
371 | /*27 */ "Keypadfacility" , |
372 | /*28 */ "LLC" , |
373 | /*29 */ "ManuData" , |
374 | /*2a */ "ManuID" , |
375 | /*2b */ "NCPI" , |
376 | /*2c */ "Reason" , |
377 | /*2d */ "Reason_B3" , |
378 | /*2e */ "Reject" , |
379 | /*2f */ "Useruserdata" |
380 | }; |
381 | |
382 | #include <linux/stdarg.h> |
383 | |
384 | /*-------------------------------------------------------*/ |
385 | static _cdebbuf *bufprint(_cdebbuf *cdb, char *fmt, ...) |
386 | { |
387 | va_list f; |
388 | size_t n, r; |
389 | |
390 | if (!cdb) |
391 | return NULL; |
392 | va_start(f, fmt); |
393 | r = cdb->size - cdb->pos; |
394 | n = vsnprintf(buf: cdb->p, size: r, fmt, args: f); |
395 | va_end(f); |
396 | if (n >= r) { |
397 | /* truncated, need bigger buffer */ |
398 | size_t ns = 2 * cdb->size; |
399 | u_char *nb; |
400 | |
401 | while ((ns - cdb->pos) <= n) |
402 | ns *= 2; |
403 | nb = kmalloc(size: ns, GFP_ATOMIC); |
404 | if (!nb) { |
405 | cdebbuf_free(cdb); |
406 | return NULL; |
407 | } |
408 | memcpy(nb, cdb->buf, cdb->pos); |
409 | kfree(objp: cdb->buf); |
410 | nb[cdb->pos] = 0; |
411 | cdb->buf = nb; |
412 | cdb->p = cdb->buf + cdb->pos; |
413 | cdb->size = ns; |
414 | va_start(f, fmt); |
415 | r = cdb->size - cdb->pos; |
416 | n = vsnprintf(buf: cdb->p, size: r, fmt, args: f); |
417 | va_end(f); |
418 | } |
419 | cdb->p += n; |
420 | cdb->pos += n; |
421 | return cdb; |
422 | } |
423 | |
424 | static _cdebbuf *printstructlen(_cdebbuf *cdb, u8 *m, unsigned len) |
425 | { |
426 | unsigned hex = 0; |
427 | |
428 | if (!cdb) |
429 | return NULL; |
430 | for (; len; len--, m++) |
431 | if (isalnum(*m) || *m == ' ') { |
432 | if (hex) |
433 | cdb = bufprint(cdb, fmt: ">" ); |
434 | cdb = bufprint(cdb, fmt: "%c" , *m); |
435 | hex = 0; |
436 | } else { |
437 | if (!hex) |
438 | cdb = bufprint(cdb, fmt: "<%02x" , *m); |
439 | else |
440 | cdb = bufprint(cdb, fmt: " %02x" , *m); |
441 | hex = 1; |
442 | } |
443 | if (hex) |
444 | cdb = bufprint(cdb, fmt: ">" ); |
445 | return cdb; |
446 | } |
447 | |
448 | static _cdebbuf *printstruct(_cdebbuf *cdb, u8 *m) |
449 | { |
450 | unsigned len; |
451 | |
452 | if (m[0] != 0xff) { |
453 | len = m[0]; |
454 | m += 1; |
455 | } else { |
456 | len = ((u16 *) (m + 1))[0]; |
457 | m += 3; |
458 | } |
459 | cdb = printstructlen(cdb, m, len); |
460 | return cdb; |
461 | } |
462 | |
463 | /*-------------------------------------------------------*/ |
464 | #define NAME (pnames[cmsg->par[cmsg->p]]) |
465 | |
466 | static _cdebbuf *protocol_message_2_pars(_cdebbuf *cdb, _cmsg *cmsg, int level) |
467 | { |
468 | if (!cmsg->par) |
469 | return NULL; /* invalid command/subcommand */ |
470 | |
471 | for (; TYP != _CEND; cmsg->p++) { |
472 | int slen = 29 + 3 - level; |
473 | int i; |
474 | |
475 | if (!cdb) |
476 | return NULL; |
477 | cdb = bufprint(cdb, fmt: " " ); |
478 | for (i = 0; i < level - 1; i++) |
479 | cdb = bufprint(cdb, fmt: " " ); |
480 | |
481 | switch (TYP) { |
482 | case _CBYTE: |
483 | cdb = bufprint(cdb, fmt: "%-*s = 0x%x\n" , slen, NAME, *(u8 *) (cmsg->m + cmsg->l)); |
484 | cmsg->l++; |
485 | break; |
486 | case _CWORD: |
487 | cdb = bufprint(cdb, fmt: "%-*s = 0x%x\n" , slen, NAME, *(u16 *) (cmsg->m + cmsg->l)); |
488 | cmsg->l += 2; |
489 | break; |
490 | case _CDWORD: |
491 | cdb = bufprint(cdb, fmt: "%-*s = 0x%lx\n" , slen, NAME, *(u32 *) (cmsg->m + cmsg->l)); |
492 | cmsg->l += 4; |
493 | break; |
494 | case _CSTRUCT: |
495 | cdb = bufprint(cdb, fmt: "%-*s = " , slen, NAME); |
496 | if (cmsg->m[cmsg->l] == '\0') |
497 | cdb = bufprint(cdb, fmt: "default" ); |
498 | else |
499 | cdb = printstruct(cdb, m: cmsg->m + cmsg->l); |
500 | cdb = bufprint(cdb, fmt: "\n" ); |
501 | if (cmsg->m[cmsg->l] != 0xff) |
502 | cmsg->l += 1 + cmsg->m[cmsg->l]; |
503 | else |
504 | cmsg->l += 3 + *(u16 *) (cmsg->m + cmsg->l + 1); |
505 | |
506 | break; |
507 | |
508 | case _CMSTRUCT: |
509 | /*----- Metastruktur 0 -----*/ |
510 | if (cmsg->m[cmsg->l] == '\0') { |
511 | cdb = bufprint(cdb, fmt: "%-*s = default\n" , slen, NAME); |
512 | cmsg->l++; |
513 | jumpcstruct(cmsg); |
514 | } else { |
515 | char *name = NAME; |
516 | unsigned _l = cmsg->l; |
517 | cdb = bufprint(cdb, fmt: "%-*s\n" , slen, name); |
518 | cmsg->l = (cmsg->m + _l)[0] == 255 ? cmsg->l + 3 : cmsg->l + 1; |
519 | cmsg->p++; |
520 | cdb = protocol_message_2_pars(cdb, cmsg, level: level + 1); |
521 | } |
522 | break; |
523 | } |
524 | } |
525 | return cdb; |
526 | } |
527 | /*-------------------------------------------------------*/ |
528 | |
529 | static _cdebbuf *g_debbuf; |
530 | static u_long g_debbuf_lock; |
531 | static _cmsg *g_cmsg; |
532 | |
533 | static _cdebbuf *cdebbuf_alloc(void) |
534 | { |
535 | _cdebbuf *cdb; |
536 | |
537 | if (likely(!test_and_set_bit(1, &g_debbuf_lock))) { |
538 | cdb = g_debbuf; |
539 | goto init; |
540 | } else |
541 | cdb = kmalloc(size: sizeof(_cdebbuf), GFP_ATOMIC); |
542 | if (!cdb) |
543 | return NULL; |
544 | cdb->buf = kmalloc(CDEBUG_SIZE, GFP_ATOMIC); |
545 | if (!cdb->buf) { |
546 | kfree(objp: cdb); |
547 | return NULL; |
548 | } |
549 | cdb->size = CDEBUG_SIZE; |
550 | init: |
551 | cdb->buf[0] = 0; |
552 | cdb->p = cdb->buf; |
553 | cdb->pos = 0; |
554 | return cdb; |
555 | } |
556 | |
557 | /** |
558 | * cdebbuf_free() - free CAPI debug buffer |
559 | * @cdb: buffer to free |
560 | */ |
561 | |
562 | void cdebbuf_free(_cdebbuf *cdb) |
563 | { |
564 | if (likely(cdb == g_debbuf)) { |
565 | test_and_clear_bit(nr: 1, addr: &g_debbuf_lock); |
566 | return; |
567 | } |
568 | if (likely(cdb)) |
569 | kfree(objp: cdb->buf); |
570 | kfree(objp: cdb); |
571 | } |
572 | |
573 | |
574 | /** |
575 | * capi_message2str() - format CAPI 2.0 message for printing |
576 | * @msg: CAPI 2.0 message |
577 | * |
578 | * Allocates a CAPI debug buffer and fills it with a printable representation |
579 | * of the CAPI 2.0 message in @msg. |
580 | * Return value: allocated debug buffer, NULL on error |
581 | * The returned buffer should be freed by a call to cdebbuf_free() after use. |
582 | */ |
583 | |
584 | _cdebbuf *capi_message2str(u8 *msg) |
585 | { |
586 | _cdebbuf *cdb; |
587 | _cmsg *cmsg; |
588 | |
589 | cdb = cdebbuf_alloc(); |
590 | if (unlikely(!cdb)) |
591 | return NULL; |
592 | if (likely(cdb == g_debbuf)) |
593 | cmsg = g_cmsg; |
594 | else |
595 | cmsg = kmalloc(size: sizeof(_cmsg), GFP_ATOMIC); |
596 | if (unlikely(!cmsg)) { |
597 | cdebbuf_free(cdb); |
598 | return NULL; |
599 | } |
600 | cmsg->m = msg; |
601 | cmsg->l = 8; |
602 | cmsg->p = 0; |
603 | byteTRcpy(cmsg->m + 4, &cmsg->Command); |
604 | byteTRcpy(cmsg->m + 5, &cmsg->Subcommand); |
605 | cmsg->par = capi_cmd2par(cmd: cmsg->Command, subcmd: cmsg->Subcommand); |
606 | |
607 | cdb = bufprint(cdb, fmt: "%-26s ID=%03d #0x%04x LEN=%04d\n" , |
608 | capi_cmd2str(cmd: cmsg->Command, subcmd: cmsg->Subcommand), |
609 | ((unsigned short *) msg)[1], |
610 | ((unsigned short *) msg)[3], |
611 | ((unsigned short *) msg)[0]); |
612 | |
613 | cdb = protocol_message_2_pars(cdb, cmsg, level: 1); |
614 | if (unlikely(cmsg != g_cmsg)) |
615 | kfree(objp: cmsg); |
616 | return cdb; |
617 | } |
618 | |
619 | int __init cdebug_init(void) |
620 | { |
621 | g_cmsg = kmalloc(size: sizeof(_cmsg), GFP_KERNEL); |
622 | if (!g_cmsg) |
623 | return -ENOMEM; |
624 | g_debbuf = kmalloc(size: sizeof(_cdebbuf), GFP_KERNEL); |
625 | if (!g_debbuf) { |
626 | kfree(objp: g_cmsg); |
627 | return -ENOMEM; |
628 | } |
629 | g_debbuf->buf = kmalloc(CDEBUG_GSIZE, GFP_KERNEL); |
630 | if (!g_debbuf->buf) { |
631 | kfree(objp: g_cmsg); |
632 | kfree(objp: g_debbuf); |
633 | return -ENOMEM; |
634 | } |
635 | g_debbuf->size = CDEBUG_GSIZE; |
636 | g_debbuf->buf[0] = 0; |
637 | g_debbuf->p = g_debbuf->buf; |
638 | g_debbuf->pos = 0; |
639 | return 0; |
640 | } |
641 | |
642 | void cdebug_exit(void) |
643 | { |
644 | if (g_debbuf) |
645 | kfree(objp: g_debbuf->buf); |
646 | kfree(objp: g_debbuf); |
647 | kfree(objp: g_cmsg); |
648 | } |
649 | |
650 | #else /* !CONFIG_CAPI_TRACE */ |
651 | |
652 | static _cdebbuf g_debbuf = {"CONFIG_CAPI_TRACE not enabled" , NULL, 0, 0}; |
653 | |
654 | _cdebbuf *capi_message2str(u8 *msg) |
655 | { |
656 | return &g_debbuf; |
657 | } |
658 | |
659 | _cdebbuf *capi_cmsg2str(_cmsg *cmsg) |
660 | { |
661 | return &g_debbuf; |
662 | } |
663 | |
664 | void cdebbuf_free(_cdebbuf *cdb) |
665 | { |
666 | } |
667 | |
668 | int __init cdebug_init(void) |
669 | { |
670 | return 0; |
671 | } |
672 | |
673 | void cdebug_exit(void) |
674 | { |
675 | } |
676 | |
677 | #endif |
678 | |