Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 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> |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 20 | #include <linux/isdn/capiutil.h> |
Tejun Heo | 5a0e3ad | 2010-03-24 17:04:11 +0900 | [diff] [blame] | 21 | #include <linux/slab.h> |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 22 | |
| 23 | /* from CAPI2.0 DDK AVM Berlin GmbH */ |
| 24 | |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 25 | typedef struct { |
| 26 | int typ; |
| 27 | size_t off; |
| 28 | } _cdef; |
| 29 | |
| 30 | #define _CBYTE 1 |
| 31 | #define _CWORD 2 |
| 32 | #define _CDWORD 3 |
| 33 | #define _CSTRUCT 4 |
| 34 | #define _CMSTRUCT 5 |
| 35 | #define _CEND 6 |
| 36 | |
| 37 | static _cdef cdef[] = |
| 38 | { |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 39 | /*00 */ |
| 40 | {_CEND}, |
| 41 | /*01 */ |
| 42 | {_CEND}, |
| 43 | /*02 */ |
| 44 | {_CEND}, |
| 45 | /*03 */ |
| 46 | {_CDWORD, offsetof(_cmsg, adr.adrController)}, |
| 47 | /*04 */ |
| 48 | {_CMSTRUCT, offsetof(_cmsg, AdditionalInfo)}, |
| 49 | /*05 */ |
| 50 | {_CSTRUCT, offsetof(_cmsg, B1configuration)}, |
| 51 | /*06 */ |
| 52 | {_CWORD, offsetof(_cmsg, B1protocol)}, |
| 53 | /*07 */ |
| 54 | {_CSTRUCT, offsetof(_cmsg, B2configuration)}, |
| 55 | /*08 */ |
| 56 | {_CWORD, offsetof(_cmsg, B2protocol)}, |
| 57 | /*09 */ |
| 58 | {_CSTRUCT, offsetof(_cmsg, B3configuration)}, |
| 59 | /*0a */ |
| 60 | {_CWORD, offsetof(_cmsg, B3protocol)}, |
| 61 | /*0b */ |
| 62 | {_CSTRUCT, offsetof(_cmsg, BC)}, |
| 63 | /*0c */ |
| 64 | {_CSTRUCT, offsetof(_cmsg, BChannelinformation)}, |
| 65 | /*0d */ |
| 66 | {_CMSTRUCT, offsetof(_cmsg, BProtocol)}, |
| 67 | /*0e */ |
| 68 | {_CSTRUCT, offsetof(_cmsg, CalledPartyNumber)}, |
| 69 | /*0f */ |
| 70 | {_CSTRUCT, offsetof(_cmsg, CalledPartySubaddress)}, |
| 71 | /*10 */ |
| 72 | {_CSTRUCT, offsetof(_cmsg, CallingPartyNumber)}, |
| 73 | /*11 */ |
| 74 | {_CSTRUCT, offsetof(_cmsg, CallingPartySubaddress)}, |
| 75 | /*12 */ |
| 76 | {_CDWORD, offsetof(_cmsg, CIPmask)}, |
| 77 | /*13 */ |
| 78 | {_CDWORD, offsetof(_cmsg, CIPmask2)}, |
| 79 | /*14 */ |
| 80 | {_CWORD, offsetof(_cmsg, CIPValue)}, |
| 81 | /*15 */ |
| 82 | {_CDWORD, offsetof(_cmsg, Class)}, |
| 83 | /*16 */ |
| 84 | {_CSTRUCT, offsetof(_cmsg, ConnectedNumber)}, |
| 85 | /*17 */ |
| 86 | {_CSTRUCT, offsetof(_cmsg, ConnectedSubaddress)}, |
| 87 | /*18 */ |
| 88 | {_CDWORD, offsetof(_cmsg, Data)}, |
| 89 | /*19 */ |
| 90 | {_CWORD, offsetof(_cmsg, DataHandle)}, |
| 91 | /*1a */ |
| 92 | {_CWORD, offsetof(_cmsg, DataLength)}, |
| 93 | /*1b */ |
| 94 | {_CSTRUCT, offsetof(_cmsg, FacilityConfirmationParameter)}, |
| 95 | /*1c */ |
| 96 | {_CSTRUCT, offsetof(_cmsg, Facilitydataarray)}, |
| 97 | /*1d */ |
| 98 | {_CSTRUCT, offsetof(_cmsg, FacilityIndicationParameter)}, |
| 99 | /*1e */ |
| 100 | {_CSTRUCT, offsetof(_cmsg, FacilityRequestParameter)}, |
| 101 | /*1f */ |
| 102 | {_CWORD, offsetof(_cmsg, FacilitySelector)}, |
| 103 | /*20 */ |
| 104 | {_CWORD, offsetof(_cmsg, Flags)}, |
| 105 | /*21 */ |
| 106 | {_CDWORD, offsetof(_cmsg, Function)}, |
| 107 | /*22 */ |
| 108 | {_CSTRUCT, offsetof(_cmsg, HLC)}, |
| 109 | /*23 */ |
| 110 | {_CWORD, offsetof(_cmsg, Info)}, |
| 111 | /*24 */ |
| 112 | {_CSTRUCT, offsetof(_cmsg, InfoElement)}, |
| 113 | /*25 */ |
| 114 | {_CDWORD, offsetof(_cmsg, InfoMask)}, |
| 115 | /*26 */ |
| 116 | {_CWORD, offsetof(_cmsg, InfoNumber)}, |
| 117 | /*27 */ |
| 118 | {_CSTRUCT, offsetof(_cmsg, Keypadfacility)}, |
| 119 | /*28 */ |
| 120 | {_CSTRUCT, offsetof(_cmsg, LLC)}, |
| 121 | /*29 */ |
| 122 | {_CSTRUCT, offsetof(_cmsg, ManuData)}, |
| 123 | /*2a */ |
| 124 | {_CDWORD, offsetof(_cmsg, ManuID)}, |
| 125 | /*2b */ |
| 126 | {_CSTRUCT, offsetof(_cmsg, NCPI)}, |
| 127 | /*2c */ |
| 128 | {_CWORD, offsetof(_cmsg, Reason)}, |
| 129 | /*2d */ |
| 130 | {_CWORD, offsetof(_cmsg, Reason_B3)}, |
| 131 | /*2e */ |
| 132 | {_CWORD, offsetof(_cmsg, Reject)}, |
| 133 | /*2f */ |
| 134 | {_CSTRUCT, offsetof(_cmsg, Useruserdata)} |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 135 | }; |
| 136 | |
| 137 | static unsigned char *cpars[] = |
| 138 | { |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 139 | /* ALERT_REQ */ [0x01] = "\x03\x04\x0c\x27\x2f\x1c\x01\x01", |
| 140 | /* 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", |
| 141 | /* DISCONNECT_REQ */ [0x04] = "\x03\x04\x0c\x27\x2f\x1c\x01\x01", |
| 142 | /* LISTEN_REQ */ [0x05] = "\x03\x25\x12\x13\x10\x11\x01", |
| 143 | /* INFO_REQ */ [0x08] = "\x03\x0e\x04\x0c\x27\x2f\x1c\x01\x01", |
| 144 | /* FACILITY_REQ */ [0x09] = "\x03\x1f\x1e\x01", |
| 145 | /* SELECT_B_PROTOCOL_REQ */ [0x0a] = "\x03\x0d\x06\x08\x0a\x05\x07\x09\x01\x01", |
| 146 | /* CONNECT_B3_REQ */ [0x0b] = "\x03\x2b\x01", |
| 147 | /* DISCONNECT_B3_REQ */ [0x0d] = "\x03\x2b\x01", |
| 148 | /* DATA_B3_REQ */ [0x0f] = "\x03\x18\x1a\x19\x20\x01", |
| 149 | /* RESET_B3_REQ */ [0x10] = "\x03\x2b\x01", |
| 150 | /* ALERT_CONF */ [0x13] = "\x03\x23\x01", |
| 151 | /* CONNECT_CONF */ [0x14] = "\x03\x23\x01", |
| 152 | /* DISCONNECT_CONF */ [0x16] = "\x03\x23\x01", |
| 153 | /* LISTEN_CONF */ [0x17] = "\x03\x23\x01", |
| 154 | /* MANUFACTURER_REQ */ [0x18] = "\x03\x2a\x15\x21\x29\x01", |
| 155 | /* INFO_CONF */ [0x1a] = "\x03\x23\x01", |
| 156 | /* FACILITY_CONF */ [0x1b] = "\x03\x23\x1f\x1b\x01", |
| 157 | /* SELECT_B_PROTOCOL_CONF */ [0x1c] = "\x03\x23\x01", |
| 158 | /* CONNECT_B3_CONF */ [0x1d] = "\x03\x23\x01", |
| 159 | /* DISCONNECT_B3_CONF */ [0x1f] = "\x03\x23\x01", |
| 160 | /* DATA_B3_CONF */ [0x21] = "\x03\x19\x23\x01", |
| 161 | /* RESET_B3_CONF */ [0x22] = "\x03\x23\x01", |
| 162 | /* CONNECT_IND */ [0x26] = "\x03\x14\x0e\x10\x0f\x11\x0b\x28\x22\x04\x0c\x27\x2f\x1c\x01\x01", |
| 163 | /* CONNECT_ACTIVE_IND */ [0x27] = "\x03\x16\x17\x28\x01", |
| 164 | /* DISCONNECT_IND */ [0x28] = "\x03\x2c\x01", |
| 165 | /* MANUFACTURER_CONF */ [0x2a] = "\x03\x2a\x15\x21\x29\x01", |
| 166 | /* INFO_IND */ [0x2c] = "\x03\x26\x24\x01", |
| 167 | /* FACILITY_IND */ [0x2d] = "\x03\x1f\x1d\x01", |
| 168 | /* CONNECT_B3_IND */ [0x2f] = "\x03\x2b\x01", |
| 169 | /* CONNECT_B3_ACTIVE_IND */ [0x30] = "\x03\x2b\x01", |
| 170 | /* DISCONNECT_B3_IND */ [0x31] = "\x03\x2d\x2b\x01", |
| 171 | /* DATA_B3_IND */ [0x33] = "\x03\x18\x1a\x19\x20\x01", |
| 172 | /* RESET_B3_IND */ [0x34] = "\x03\x2b\x01", |
| 173 | /* CONNECT_B3_T90_ACTIVE_IND */ [0x35] = "\x03\x2b\x01", |
| 174 | /* CONNECT_RESP */ [0x38] = "\x03\x2e\x0d\x06\x08\x0a\x05\x07\x09\x01\x16\x17\x28\x04\x0c\x27\x2f\x1c\x01\x01", |
| 175 | /* CONNECT_ACTIVE_RESP */ [0x39] = "\x03\x01", |
| 176 | /* DISCONNECT_RESP */ [0x3a] = "\x03\x01", |
| 177 | /* MANUFACTURER_IND */ [0x3c] = "\x03\x2a\x15\x21\x29\x01", |
| 178 | /* INFO_RESP */ [0x3e] = "\x03\x01", |
| 179 | /* FACILITY_RESP */ [0x3f] = "\x03\x1f\x01", |
| 180 | /* CONNECT_B3_RESP */ [0x41] = "\x03\x2e\x2b\x01", |
| 181 | /* CONNECT_B3_ACTIVE_RESP */ [0x42] = "\x03\x01", |
| 182 | /* DISCONNECT_B3_RESP */ [0x43] = "\x03\x01", |
| 183 | /* DATA_B3_RESP */ [0x45] = "\x03\x19\x01", |
| 184 | /* RESET_B3_RESP */ [0x46] = "\x03\x01", |
| 185 | /* CONNECT_B3_T90_ACTIVE_RESP */ [0x47] = "\x03\x01", |
| 186 | /* MANUFACTURER_RESP */ [0x4e] = "\x03\x2a\x15\x21\x29\x01", |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 187 | }; |
| 188 | |
| 189 | /*-------------------------------------------------------*/ |
| 190 | |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 191 | #define byteTLcpy(x, y) *(u8 *)(x) = *(u8 *)(y); |
| 192 | #define wordTLcpy(x, y) *(u16 *)(x) = *(u16 *)(y); |
| 193 | #define dwordTLcpy(x, y) memcpy(x, y, 4); |
| 194 | #define structTLcpy(x, y, l) memcpy(x, y, l) |
| 195 | #define structTLcpyovl(x, y, l) memmove(x, y, l) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 196 | |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 197 | #define byteTRcpy(x, y) *(u8 *)(y) = *(u8 *)(x); |
| 198 | #define wordTRcpy(x, y) *(u16 *)(y) = *(u16 *)(x); |
| 199 | #define dwordTRcpy(x, y) memcpy(y, x, 4); |
| 200 | #define structTRcpy(x, y, l) memcpy(y, x, l) |
| 201 | #define structTRcpyovl(x, y, l) memmove(y, x, l) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 202 | |
| 203 | /*-------------------------------------------------------*/ |
| 204 | static unsigned command_2_index(unsigned c, unsigned sc) |
| 205 | { |
| 206 | if (c & 0x80) |
| 207 | c = 0x9 + (c & 0x0f); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 208 | else if (c == 0x41) |
| 209 | c = 0x9 + 0x1; |
Tilman Schmidt | 5362247 | 2014-10-11 13:46:30 +0200 | [diff] [blame] | 210 | if (c > 0x18) |
| 211 | c = 0x00; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 212 | return (sc & 3) * (0x9 + 0x9) + c; |
| 213 | } |
| 214 | |
Tilman Schmidt | 854d23b | 2014-10-11 13:46:30 +0200 | [diff] [blame] | 215 | /** |
| 216 | * capi_cmd2par() - find parameter string for CAPI 2.0 command/subcommand |
| 217 | * @cmd: command number |
| 218 | * @subcmd: subcommand number |
| 219 | * |
| 220 | * Return value: static string, NULL if command/subcommand unknown |
| 221 | */ |
| 222 | |
| 223 | static unsigned char *capi_cmd2par(u8 cmd, u8 subcmd) |
| 224 | { |
| 225 | return cpars[command_2_index(cmd, subcmd)]; |
| 226 | } |
| 227 | |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 228 | /*-------------------------------------------------------*/ |
| 229 | #define TYP (cdef[cmsg->par[cmsg->p]].typ) |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 230 | #define OFF (((u8 *)cmsg) + cdef[cmsg->par[cmsg->p]].off) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 231 | |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 232 | static void jumpcstruct(_cmsg *cmsg) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 233 | { |
| 234 | unsigned layer; |
| 235 | for (cmsg->p++, layer = 1; layer;) { |
| 236 | /* $$$$$ assert (cmsg->p); */ |
| 237 | cmsg->p++; |
| 238 | switch (TYP) { |
| 239 | case _CMSTRUCT: |
| 240 | layer++; |
| 241 | break; |
| 242 | case _CEND: |
| 243 | layer--; |
| 244 | break; |
| 245 | } |
| 246 | } |
| 247 | } |
| 248 | /*-------------------------------------------------------*/ |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 249 | static void pars_2_message(_cmsg *cmsg) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 250 | { |
| 251 | |
| 252 | for (; TYP != _CEND; cmsg->p++) { |
| 253 | switch (TYP) { |
| 254 | case _CBYTE: |
| 255 | byteTLcpy(cmsg->m + cmsg->l, OFF); |
| 256 | cmsg->l++; |
| 257 | break; |
| 258 | case _CWORD: |
| 259 | wordTLcpy(cmsg->m + cmsg->l, OFF); |
| 260 | cmsg->l += 2; |
| 261 | break; |
| 262 | case _CDWORD: |
| 263 | dwordTLcpy(cmsg->m + cmsg->l, OFF); |
| 264 | cmsg->l += 4; |
| 265 | break; |
| 266 | case _CSTRUCT: |
Harvey Harrison | 2f9e9b6 | 2008-04-28 02:14:37 -0700 | [diff] [blame] | 267 | if (*(u8 **) OFF == NULL) { |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 268 | *(cmsg->m + cmsg->l) = '\0'; |
| 269 | cmsg->l++; |
| 270 | } else if (**(_cstruct *) OFF != 0xff) { |
| 271 | structTLcpy(cmsg->m + cmsg->l, *(_cstruct *) OFF, 1 + **(_cstruct *) OFF); |
| 272 | cmsg->l += 1 + **(_cstruct *) OFF; |
| 273 | } else { |
| 274 | _cstruct s = *(_cstruct *) OFF; |
| 275 | structTLcpy(cmsg->m + cmsg->l, s, 3 + *(u16 *) (s + 1)); |
| 276 | cmsg->l += 3 + *(u16 *) (s + 1); |
| 277 | } |
| 278 | break; |
| 279 | case _CMSTRUCT: |
| 280 | /*----- Metastruktur 0 -----*/ |
| 281 | if (*(_cmstruct *) OFF == CAPI_DEFAULT) { |
| 282 | *(cmsg->m + cmsg->l) = '\0'; |
| 283 | cmsg->l++; |
| 284 | jumpcstruct(cmsg); |
| 285 | } |
| 286 | /*----- Metastruktur wird composed -----*/ |
| 287 | else { |
| 288 | unsigned _l = cmsg->l; |
| 289 | unsigned _ls; |
| 290 | cmsg->l++; |
| 291 | cmsg->p++; |
| 292 | pars_2_message(cmsg); |
| 293 | _ls = cmsg->l - _l - 1; |
| 294 | if (_ls < 255) |
| 295 | (cmsg->m + _l)[0] = (u8) _ls; |
| 296 | else { |
| 297 | structTLcpyovl(cmsg->m + _l + 3, cmsg->m + _l + 1, _ls); |
| 298 | (cmsg->m + _l)[0] = 0xff; |
| 299 | wordTLcpy(cmsg->m + _l + 1, &_ls); |
| 300 | } |
| 301 | } |
| 302 | break; |
| 303 | } |
| 304 | } |
| 305 | } |
| 306 | |
Tilman Schmidt | 4793d15 | 2009-06-07 09:09:23 +0000 | [diff] [blame] | 307 | /** |
| 308 | * capi_cmsg2message() - assemble CAPI 2.0 message from _cmsg structure |
| 309 | * @cmsg: _cmsg structure |
| 310 | * @msg: buffer for assembled message |
| 311 | * |
| 312 | * Return value: 0 for success |
| 313 | */ |
| 314 | |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 315 | unsigned capi_cmsg2message(_cmsg *cmsg, u8 *msg) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 316 | { |
| 317 | cmsg->m = msg; |
| 318 | cmsg->l = 8; |
| 319 | cmsg->p = 0; |
Tilman Schmidt | 854d23b | 2014-10-11 13:46:30 +0200 | [diff] [blame] | 320 | cmsg->par = capi_cmd2par(cmsg->Command, cmsg->Subcommand); |
Tilman Schmidt | 5510ab1 | 2014-10-11 13:46:30 +0200 | [diff] [blame] | 321 | if (!cmsg->par) |
| 322 | return 1; /* invalid command/subcommand */ |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 323 | |
| 324 | pars_2_message(cmsg); |
| 325 | |
| 326 | wordTLcpy(msg + 0, &cmsg->l); |
| 327 | byteTLcpy(cmsg->m + 4, &cmsg->Command); |
| 328 | byteTLcpy(cmsg->m + 5, &cmsg->Subcommand); |
| 329 | wordTLcpy(cmsg->m + 2, &cmsg->ApplId); |
| 330 | wordTLcpy(cmsg->m + 6, &cmsg->Messagenumber); |
| 331 | |
| 332 | return 0; |
| 333 | } |
| 334 | |
| 335 | /*-------------------------------------------------------*/ |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 336 | static void message_2_pars(_cmsg *cmsg) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 337 | { |
| 338 | for (; TYP != _CEND; cmsg->p++) { |
| 339 | |
| 340 | switch (TYP) { |
| 341 | case _CBYTE: |
| 342 | byteTRcpy(cmsg->m + cmsg->l, OFF); |
| 343 | cmsg->l++; |
| 344 | break; |
| 345 | case _CWORD: |
| 346 | wordTRcpy(cmsg->m + cmsg->l, OFF); |
| 347 | cmsg->l += 2; |
| 348 | break; |
| 349 | case _CDWORD: |
| 350 | dwordTRcpy(cmsg->m + cmsg->l, OFF); |
| 351 | cmsg->l += 4; |
| 352 | break; |
| 353 | case _CSTRUCT: |
| 354 | *(u8 **) OFF = cmsg->m + cmsg->l; |
| 355 | |
| 356 | if (cmsg->m[cmsg->l] != 0xff) |
| 357 | cmsg->l += 1 + cmsg->m[cmsg->l]; |
| 358 | else |
| 359 | cmsg->l += 3 + *(u16 *) (cmsg->m + cmsg->l + 1); |
| 360 | break; |
| 361 | case _CMSTRUCT: |
| 362 | /*----- Metastruktur 0 -----*/ |
| 363 | if (cmsg->m[cmsg->l] == '\0') { |
| 364 | *(_cmstruct *) OFF = CAPI_DEFAULT; |
| 365 | cmsg->l++; |
| 366 | jumpcstruct(cmsg); |
| 367 | } else { |
| 368 | unsigned _l = cmsg->l; |
| 369 | *(_cmstruct *) OFF = CAPI_COMPOSE; |
| 370 | cmsg->l = (cmsg->m + _l)[0] == 255 ? cmsg->l + 3 : cmsg->l + 1; |
| 371 | cmsg->p++; |
| 372 | message_2_pars(cmsg); |
| 373 | } |
| 374 | break; |
| 375 | } |
| 376 | } |
| 377 | } |
| 378 | |
Tilman Schmidt | 4793d15 | 2009-06-07 09:09:23 +0000 | [diff] [blame] | 379 | /** |
| 380 | * capi_message2cmsg() - disassemble CAPI 2.0 message into _cmsg structure |
| 381 | * @cmsg: _cmsg structure |
| 382 | * @msg: buffer for assembled message |
| 383 | * |
| 384 | * Return value: 0 for success |
| 385 | */ |
| 386 | |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 387 | unsigned capi_message2cmsg(_cmsg *cmsg, u8 *msg) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 388 | { |
| 389 | memset(cmsg, 0, sizeof(_cmsg)); |
| 390 | cmsg->m = msg; |
| 391 | cmsg->l = 8; |
| 392 | cmsg->p = 0; |
| 393 | byteTRcpy(cmsg->m + 4, &cmsg->Command); |
| 394 | byteTRcpy(cmsg->m + 5, &cmsg->Subcommand); |
Tilman Schmidt | 854d23b | 2014-10-11 13:46:30 +0200 | [diff] [blame] | 395 | cmsg->par = capi_cmd2par(cmsg->Command, cmsg->Subcommand); |
Tilman Schmidt | 5510ab1 | 2014-10-11 13:46:30 +0200 | [diff] [blame] | 396 | if (!cmsg->par) |
| 397 | return 1; /* invalid command/subcommand */ |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 398 | |
| 399 | message_2_pars(cmsg); |
| 400 | |
| 401 | wordTRcpy(msg + 0, &cmsg->l); |
| 402 | wordTRcpy(cmsg->m + 2, &cmsg->ApplId); |
| 403 | wordTRcpy(cmsg->m + 6, &cmsg->Messagenumber); |
| 404 | |
| 405 | return 0; |
| 406 | } |
| 407 | |
Tilman Schmidt | 4793d15 | 2009-06-07 09:09:23 +0000 | [diff] [blame] | 408 | /** |
| 409 | * capi_cmsg_header() - initialize header part of _cmsg structure |
| 410 | * @cmsg: _cmsg structure |
| 411 | * @_ApplId: ApplID field value |
| 412 | * @_Command: Command field value |
| 413 | * @_Subcommand: Subcommand field value |
| 414 | * @_Messagenumber: Message Number field value |
| 415 | * @_Controller: Controller/PLCI/NCCI field value |
| 416 | * |
| 417 | * Return value: 0 for success |
| 418 | */ |
| 419 | |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 420 | unsigned capi_cmsg_header(_cmsg *cmsg, u16 _ApplId, |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 421 | u8 _Command, u8 _Subcommand, |
| 422 | u16 _Messagenumber, u32 _Controller) |
| 423 | { |
| 424 | memset(cmsg, 0, sizeof(_cmsg)); |
| 425 | cmsg->ApplId = _ApplId; |
| 426 | cmsg->Command = _Command; |
| 427 | cmsg->Subcommand = _Subcommand; |
| 428 | cmsg->Messagenumber = _Messagenumber; |
| 429 | cmsg->adr.adrController = _Controller; |
| 430 | return 0; |
| 431 | } |
| 432 | |
| 433 | /*-------------------------------------------------------*/ |
| 434 | |
| 435 | static char *mnames[] = |
| 436 | { |
| 437 | [0x01] = "ALERT_REQ", |
| 438 | [0x02] = "CONNECT_REQ", |
| 439 | [0x04] = "DISCONNECT_REQ", |
| 440 | [0x05] = "LISTEN_REQ", |
| 441 | [0x08] = "INFO_REQ", |
| 442 | [0x09] = "FACILITY_REQ", |
| 443 | [0x0a] = "SELECT_B_PROTOCOL_REQ", |
| 444 | [0x0b] = "CONNECT_B3_REQ", |
| 445 | [0x0d] = "DISCONNECT_B3_REQ", |
| 446 | [0x0f] = "DATA_B3_REQ", |
| 447 | [0x10] = "RESET_B3_REQ", |
| 448 | [0x13] = "ALERT_CONF", |
| 449 | [0x14] = "CONNECT_CONF", |
| 450 | [0x16] = "DISCONNECT_CONF", |
| 451 | [0x17] = "LISTEN_CONF", |
| 452 | [0x18] = "MANUFACTURER_REQ", |
| 453 | [0x1a] = "INFO_CONF", |
| 454 | [0x1b] = "FACILITY_CONF", |
| 455 | [0x1c] = "SELECT_B_PROTOCOL_CONF", |
| 456 | [0x1d] = "CONNECT_B3_CONF", |
| 457 | [0x1f] = "DISCONNECT_B3_CONF", |
| 458 | [0x21] = "DATA_B3_CONF", |
| 459 | [0x22] = "RESET_B3_CONF", |
| 460 | [0x26] = "CONNECT_IND", |
| 461 | [0x27] = "CONNECT_ACTIVE_IND", |
| 462 | [0x28] = "DISCONNECT_IND", |
| 463 | [0x2a] = "MANUFACTURER_CONF", |
| 464 | [0x2c] = "INFO_IND", |
| 465 | [0x2d] = "FACILITY_IND", |
| 466 | [0x2f] = "CONNECT_B3_IND", |
| 467 | [0x30] = "CONNECT_B3_ACTIVE_IND", |
| 468 | [0x31] = "DISCONNECT_B3_IND", |
| 469 | [0x33] = "DATA_B3_IND", |
| 470 | [0x34] = "RESET_B3_IND", |
| 471 | [0x35] = "CONNECT_B3_T90_ACTIVE_IND", |
| 472 | [0x38] = "CONNECT_RESP", |
| 473 | [0x39] = "CONNECT_ACTIVE_RESP", |
| 474 | [0x3a] = "DISCONNECT_RESP", |
| 475 | [0x3c] = "MANUFACTURER_IND", |
| 476 | [0x3e] = "INFO_RESP", |
| 477 | [0x3f] = "FACILITY_RESP", |
| 478 | [0x41] = "CONNECT_B3_RESP", |
| 479 | [0x42] = "CONNECT_B3_ACTIVE_RESP", |
| 480 | [0x43] = "DISCONNECT_B3_RESP", |
| 481 | [0x45] = "DATA_B3_RESP", |
| 482 | [0x46] = "RESET_B3_RESP", |
| 483 | [0x47] = "CONNECT_B3_T90_ACTIVE_RESP", |
| 484 | [0x4e] = "MANUFACTURER_RESP" |
| 485 | }; |
| 486 | |
Tilman Schmidt | 4793d15 | 2009-06-07 09:09:23 +0000 | [diff] [blame] | 487 | /** |
| 488 | * capi_cmd2str() - convert CAPI 2.0 command/subcommand number to name |
| 489 | * @cmd: command number |
| 490 | * @subcmd: subcommand number |
| 491 | * |
Tilman Schmidt | 340184b | 2014-10-11 13:46:30 +0200 | [diff] [blame] | 492 | * Return value: static string |
Tilman Schmidt | 4793d15 | 2009-06-07 09:09:23 +0000 | [diff] [blame] | 493 | */ |
| 494 | |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 495 | char *capi_cmd2str(u8 cmd, u8 subcmd) |
| 496 | { |
Tilman Schmidt | 340184b | 2014-10-11 13:46:30 +0200 | [diff] [blame] | 497 | char *result; |
| 498 | |
| 499 | result = mnames[command_2_index(cmd, subcmd)]; |
| 500 | if (result == NULL) |
| 501 | result = "INVALID_COMMAND"; |
| 502 | return result; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 503 | } |
| 504 | |
| 505 | |
| 506 | /*-------------------------------------------------------*/ |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 507 | |
| 508 | #ifdef CONFIG_CAPI_TRACE |
| 509 | |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 510 | /*-------------------------------------------------------*/ |
| 511 | |
| 512 | static char *pnames[] = |
| 513 | { |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 514 | /*00 */ NULL, |
| 515 | /*01 */ NULL, |
| 516 | /*02 */ NULL, |
| 517 | /*03 */ "Controller/PLCI/NCCI", |
| 518 | /*04 */ "AdditionalInfo", |
| 519 | /*05 */ "B1configuration", |
| 520 | /*06 */ "B1protocol", |
| 521 | /*07 */ "B2configuration", |
| 522 | /*08 */ "B2protocol", |
| 523 | /*09 */ "B3configuration", |
| 524 | /*0a */ "B3protocol", |
| 525 | /*0b */ "BC", |
| 526 | /*0c */ "BChannelinformation", |
| 527 | /*0d */ "BProtocol", |
| 528 | /*0e */ "CalledPartyNumber", |
| 529 | /*0f */ "CalledPartySubaddress", |
| 530 | /*10 */ "CallingPartyNumber", |
| 531 | /*11 */ "CallingPartySubaddress", |
| 532 | /*12 */ "CIPmask", |
| 533 | /*13 */ "CIPmask2", |
| 534 | /*14 */ "CIPValue", |
| 535 | /*15 */ "Class", |
| 536 | /*16 */ "ConnectedNumber", |
| 537 | /*17 */ "ConnectedSubaddress", |
| 538 | /*18 */ "Data32", |
| 539 | /*19 */ "DataHandle", |
| 540 | /*1a */ "DataLength", |
| 541 | /*1b */ "FacilityConfirmationParameter", |
| 542 | /*1c */ "Facilitydataarray", |
| 543 | /*1d */ "FacilityIndicationParameter", |
| 544 | /*1e */ "FacilityRequestParameter", |
| 545 | /*1f */ "FacilitySelector", |
| 546 | /*20 */ "Flags", |
| 547 | /*21 */ "Function", |
| 548 | /*22 */ "HLC", |
| 549 | /*23 */ "Info", |
| 550 | /*24 */ "InfoElement", |
| 551 | /*25 */ "InfoMask", |
| 552 | /*26 */ "InfoNumber", |
| 553 | /*27 */ "Keypadfacility", |
| 554 | /*28 */ "LLC", |
| 555 | /*29 */ "ManuData", |
| 556 | /*2a */ "ManuID", |
| 557 | /*2b */ "NCPI", |
| 558 | /*2c */ "Reason", |
| 559 | /*2d */ "Reason_B3", |
| 560 | /*2e */ "Reject", |
| 561 | /*2f */ "Useruserdata" |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 562 | }; |
| 563 | |
| 564 | |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 565 | |
| 566 | #include <stdarg.h> |
| 567 | |
| 568 | /*-------------------------------------------------------*/ |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 569 | static _cdebbuf *bufprint(_cdebbuf *cdb, char *fmt, ...) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 570 | { |
| 571 | va_list f; |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 572 | size_t n, r; |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 573 | |
| 574 | if (!cdb) |
| 575 | return NULL; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 576 | va_start(f, fmt); |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 577 | r = cdb->size - cdb->pos; |
| 578 | n = vsnprintf(cdb->p, r, fmt, f); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 579 | va_end(f); |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 580 | if (n >= r) { |
| 581 | /* truncated, need bigger buffer */ |
| 582 | size_t ns = 2 * cdb->size; |
| 583 | u_char *nb; |
| 584 | |
| 585 | while ((ns - cdb->pos) <= n) |
| 586 | ns *= 2; |
| 587 | nb = kmalloc(ns, GFP_ATOMIC); |
| 588 | if (!nb) { |
| 589 | cdebbuf_free(cdb); |
| 590 | return NULL; |
| 591 | } |
| 592 | memcpy(nb, cdb->buf, cdb->pos); |
| 593 | kfree(cdb->buf); |
| 594 | nb[cdb->pos] = 0; |
| 595 | cdb->buf = nb; |
| 596 | cdb->p = cdb->buf + cdb->pos; |
| 597 | cdb->size = ns; |
| 598 | va_start(f, fmt); |
| 599 | r = cdb->size - cdb->pos; |
| 600 | n = vsnprintf(cdb->p, r, fmt, f); |
| 601 | va_end(f); |
| 602 | } |
| 603 | cdb->p += n; |
| 604 | cdb->pos += n; |
| 605 | return cdb; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 606 | } |
| 607 | |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 608 | static _cdebbuf *printstructlen(_cdebbuf *cdb, u8 *m, unsigned len) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 609 | { |
| 610 | unsigned hex = 0; |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 611 | |
| 612 | if (!cdb) |
| 613 | return NULL; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 614 | for (; len; len--, m++) |
| 615 | if (isalnum(*m) || *m == ' ') { |
| 616 | if (hex) |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 617 | cdb = bufprint(cdb, ">"); |
| 618 | cdb = bufprint(cdb, "%c", *m); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 619 | hex = 0; |
| 620 | } else { |
| 621 | if (!hex) |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 622 | cdb = bufprint(cdb, "<%02x", *m); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 623 | else |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 624 | cdb = bufprint(cdb, " %02x", *m); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 625 | hex = 1; |
| 626 | } |
| 627 | if (hex) |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 628 | cdb = bufprint(cdb, ">"); |
| 629 | return cdb; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 630 | } |
| 631 | |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 632 | static _cdebbuf *printstruct(_cdebbuf *cdb, u8 *m) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 633 | { |
| 634 | unsigned len; |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 635 | |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 636 | if (m[0] != 0xff) { |
| 637 | len = m[0]; |
| 638 | m += 1; |
| 639 | } else { |
| 640 | len = ((u16 *) (m + 1))[0]; |
| 641 | m += 3; |
| 642 | } |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 643 | cdb = printstructlen(cdb, m, len); |
| 644 | return cdb; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 645 | } |
| 646 | |
| 647 | /*-------------------------------------------------------*/ |
| 648 | #define NAME (pnames[cmsg->par[cmsg->p]]) |
| 649 | |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 650 | static _cdebbuf *protocol_message_2_pars(_cdebbuf *cdb, _cmsg *cmsg, int level) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 651 | { |
Tilman Schmidt | 5510ab1 | 2014-10-11 13:46:30 +0200 | [diff] [blame] | 652 | if (!cmsg->par) |
| 653 | return NULL; /* invalid command/subcommand */ |
| 654 | |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 655 | for (; TYP != _CEND; cmsg->p++) { |
| 656 | int slen = 29 + 3 - level; |
| 657 | int i; |
| 658 | |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 659 | if (!cdb) |
| 660 | return NULL; |
| 661 | cdb = bufprint(cdb, " "); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 662 | for (i = 0; i < level - 1; i++) |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 663 | cdb = bufprint(cdb, " "); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 664 | |
| 665 | switch (TYP) { |
| 666 | case _CBYTE: |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 667 | cdb = bufprint(cdb, "%-*s = 0x%x\n", slen, NAME, *(u8 *) (cmsg->m + cmsg->l)); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 668 | cmsg->l++; |
| 669 | break; |
| 670 | case _CWORD: |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 671 | cdb = bufprint(cdb, "%-*s = 0x%x\n", slen, NAME, *(u16 *) (cmsg->m + cmsg->l)); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 672 | cmsg->l += 2; |
| 673 | break; |
| 674 | case _CDWORD: |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 675 | cdb = bufprint(cdb, "%-*s = 0x%lx\n", slen, NAME, *(u32 *) (cmsg->m + cmsg->l)); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 676 | cmsg->l += 4; |
| 677 | break; |
| 678 | case _CSTRUCT: |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 679 | cdb = bufprint(cdb, "%-*s = ", slen, NAME); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 680 | if (cmsg->m[cmsg->l] == '\0') |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 681 | cdb = bufprint(cdb, "default"); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 682 | else |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 683 | cdb = printstruct(cdb, cmsg->m + cmsg->l); |
| 684 | cdb = bufprint(cdb, "\n"); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 685 | if (cmsg->m[cmsg->l] != 0xff) |
| 686 | cmsg->l += 1 + cmsg->m[cmsg->l]; |
| 687 | else |
| 688 | cmsg->l += 3 + *(u16 *) (cmsg->m + cmsg->l + 1); |
| 689 | |
| 690 | break; |
| 691 | |
| 692 | case _CMSTRUCT: |
| 693 | /*----- Metastruktur 0 -----*/ |
| 694 | if (cmsg->m[cmsg->l] == '\0') { |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 695 | cdb = bufprint(cdb, "%-*s = default\n", slen, NAME); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 696 | cmsg->l++; |
| 697 | jumpcstruct(cmsg); |
| 698 | } else { |
| 699 | char *name = NAME; |
| 700 | unsigned _l = cmsg->l; |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 701 | cdb = bufprint(cdb, "%-*s\n", slen, name); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 702 | cmsg->l = (cmsg->m + _l)[0] == 255 ? cmsg->l + 3 : cmsg->l + 1; |
| 703 | cmsg->p++; |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 704 | cdb = protocol_message_2_pars(cdb, cmsg, level + 1); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 705 | } |
| 706 | break; |
| 707 | } |
| 708 | } |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 709 | return cdb; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 710 | } |
| 711 | /*-------------------------------------------------------*/ |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 712 | |
| 713 | static _cdebbuf *g_debbuf; |
| 714 | static u_long g_debbuf_lock; |
| 715 | static _cmsg *g_cmsg; |
| 716 | |
Adrian Bunk | 3a3a51d | 2007-05-08 00:32:33 -0700 | [diff] [blame] | 717 | static _cdebbuf *cdebbuf_alloc(void) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 718 | { |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 719 | _cdebbuf *cdb; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 720 | |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 721 | if (likely(!test_and_set_bit(1, &g_debbuf_lock))) { |
| 722 | cdb = g_debbuf; |
| 723 | goto init; |
| 724 | } else |
| 725 | cdb = kmalloc(sizeof(_cdebbuf), GFP_ATOMIC); |
| 726 | if (!cdb) |
| 727 | return NULL; |
| 728 | cdb->buf = kmalloc(CDEBUG_SIZE, GFP_ATOMIC); |
| 729 | if (!cdb->buf) { |
| 730 | kfree(cdb); |
| 731 | return NULL; |
| 732 | } |
| 733 | cdb->size = CDEBUG_SIZE; |
| 734 | init: |
| 735 | cdb->buf[0] = 0; |
| 736 | cdb->p = cdb->buf; |
| 737 | cdb->pos = 0; |
| 738 | return cdb; |
| 739 | } |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 740 | |
Tilman Schmidt | 4793d15 | 2009-06-07 09:09:23 +0000 | [diff] [blame] | 741 | /** |
| 742 | * cdebbuf_free() - free CAPI debug buffer |
| 743 | * @cdb: buffer to free |
| 744 | */ |
| 745 | |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 746 | void cdebbuf_free(_cdebbuf *cdb) |
| 747 | { |
| 748 | if (likely(cdb == g_debbuf)) { |
| 749 | test_and_clear_bit(1, &g_debbuf_lock); |
| 750 | return; |
| 751 | } |
| 752 | if (likely(cdb)) |
| 753 | kfree(cdb->buf); |
| 754 | kfree(cdb); |
| 755 | } |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 756 | |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 757 | |
Tilman Schmidt | 4793d15 | 2009-06-07 09:09:23 +0000 | [diff] [blame] | 758 | /** |
| 759 | * capi_message2str() - format CAPI 2.0 message for printing |
| 760 | * @msg: CAPI 2.0 message |
| 761 | * |
| 762 | * Allocates a CAPI debug buffer and fills it with a printable representation |
| 763 | * of the CAPI 2.0 message in @msg. |
| 764 | * Return value: allocated debug buffer, NULL on error |
| 765 | * The returned buffer should be freed by a call to cdebbuf_free() after use. |
| 766 | */ |
| 767 | |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 768 | _cdebbuf *capi_message2str(u8 *msg) |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 769 | { |
| 770 | _cdebbuf *cdb; |
| 771 | _cmsg *cmsg; |
| 772 | |
| 773 | cdb = cdebbuf_alloc(); |
| 774 | if (unlikely(!cdb)) |
| 775 | return NULL; |
| 776 | if (likely(cdb == g_debbuf)) |
| 777 | cmsg = g_cmsg; |
| 778 | else |
| 779 | cmsg = kmalloc(sizeof(_cmsg), GFP_ATOMIC); |
| 780 | if (unlikely(!cmsg)) { |
| 781 | cdebbuf_free(cdb); |
| 782 | return NULL; |
| 783 | } |
| 784 | cmsg->m = msg; |
| 785 | cmsg->l = 8; |
| 786 | cmsg->p = 0; |
| 787 | byteTRcpy(cmsg->m + 4, &cmsg->Command); |
| 788 | byteTRcpy(cmsg->m + 5, &cmsg->Subcommand); |
Tilman Schmidt | 854d23b | 2014-10-11 13:46:30 +0200 | [diff] [blame] | 789 | cmsg->par = capi_cmd2par(cmsg->Command, cmsg->Subcommand); |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 790 | |
| 791 | cdb = bufprint(cdb, "%-26s ID=%03d #0x%04x LEN=%04d\n", |
Tilman Schmidt | 854d23b | 2014-10-11 13:46:30 +0200 | [diff] [blame] | 792 | capi_cmd2str(cmsg->Command, cmsg->Subcommand), |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 793 | ((unsigned short *) msg)[1], |
| 794 | ((unsigned short *) msg)[3], |
| 795 | ((unsigned short *) msg)[0]); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 796 | |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 797 | cdb = protocol_message_2_pars(cdb, cmsg, 1); |
| 798 | if (unlikely(cmsg != g_cmsg)) |
| 799 | kfree(cmsg); |
| 800 | return cdb; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 801 | } |
| 802 | |
Tilman Schmidt | 4793d15 | 2009-06-07 09:09:23 +0000 | [diff] [blame] | 803 | /** |
| 804 | * capi_cmsg2str() - format _cmsg structure for printing |
| 805 | * @cmsg: _cmsg structure |
| 806 | * |
| 807 | * Allocates a CAPI debug buffer and fills it with a printable representation |
| 808 | * of the CAPI 2.0 message stored in @cmsg by a previous call to |
| 809 | * capi_cmsg2message() or capi_message2cmsg(). |
| 810 | * Return value: allocated debug buffer, NULL on error |
| 811 | * The returned buffer should be freed by a call to cdebbuf_free() after use. |
| 812 | */ |
| 813 | |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 814 | _cdebbuf *capi_cmsg2str(_cmsg *cmsg) |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 815 | { |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 816 | _cdebbuf *cdb; |
| 817 | |
Tilman Schmidt | 7d31acd | 2009-06-07 09:09:24 +0000 | [diff] [blame] | 818 | if (!cmsg->m) |
| 819 | return NULL; /* no message */ |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 820 | cdb = cdebbuf_alloc(); |
| 821 | if (!cdb) |
| 822 | return NULL; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 823 | cmsg->l = 8; |
| 824 | cmsg->p = 0; |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 825 | cdb = bufprint(cdb, "%s ID=%03d #0x%04x LEN=%04d\n", |
Tilman Schmidt | 854d23b | 2014-10-11 13:46:30 +0200 | [diff] [blame] | 826 | capi_cmd2str(cmsg->Command, cmsg->Subcommand), |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 827 | ((u16 *) cmsg->m)[1], |
| 828 | ((u16 *) cmsg->m)[3], |
| 829 | ((u16 *) cmsg->m)[0]); |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 830 | cdb = protocol_message_2_pars(cdb, cmsg, 1); |
| 831 | return cdb; |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 832 | } |
| 833 | |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 834 | int __init cdebug_init(void) |
| 835 | { |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 836 | g_cmsg = kmalloc(sizeof(_cmsg), GFP_KERNEL); |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 837 | if (!g_cmsg) |
Marcin Slusarz | 066b211 | 2008-05-14 23:30:06 -0700 | [diff] [blame] | 838 | return -ENOMEM; |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 839 | g_debbuf = kmalloc(sizeof(_cdebbuf), GFP_KERNEL); |
| 840 | if (!g_debbuf) { |
| 841 | kfree(g_cmsg); |
Marcin Slusarz | 066b211 | 2008-05-14 23:30:06 -0700 | [diff] [blame] | 842 | return -ENOMEM; |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 843 | } |
| 844 | g_debbuf->buf = kmalloc(CDEBUG_GSIZE, GFP_KERNEL); |
| 845 | if (!g_debbuf->buf) { |
| 846 | kfree(g_cmsg); |
| 847 | kfree(g_debbuf); |
Joe Perches | a419aef | 2009-08-18 11:18:35 -0700 | [diff] [blame] | 848 | return -ENOMEM; |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 849 | } |
| 850 | g_debbuf->size = CDEBUG_GSIZE; |
| 851 | g_debbuf->buf[0] = 0; |
| 852 | g_debbuf->p = g_debbuf->buf; |
| 853 | g_debbuf->pos = 0; |
| 854 | return 0; |
| 855 | } |
| 856 | |
| 857 | void __exit cdebug_exit(void) |
| 858 | { |
| 859 | if (g_debbuf) |
| 860 | kfree(g_debbuf->buf); |
| 861 | kfree(g_debbuf); |
| 862 | kfree(g_cmsg); |
| 863 | } |
| 864 | |
| 865 | #else /* !CONFIG_CAPI_TRACE */ |
| 866 | |
| 867 | static _cdebbuf g_debbuf = {"CONFIG_CAPI_TRACE not enabled", NULL, 0, 0}; |
| 868 | |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 869 | _cdebbuf *capi_message2str(u8 *msg) |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 870 | { |
| 871 | return &g_debbuf; |
| 872 | } |
| 873 | |
Joe Perches | 475be4d | 2012-02-19 19:52:38 -0800 | [diff] [blame] | 874 | _cdebbuf *capi_cmsg2str(_cmsg *cmsg) |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 875 | { |
| 876 | return &g_debbuf; |
| 877 | } |
| 878 | |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 879 | void cdebbuf_free(_cdebbuf *cdb) |
| 880 | { |
| 881 | } |
| 882 | |
| 883 | int __init cdebug_init(void) |
| 884 | { |
| 885 | return 0; |
| 886 | } |
| 887 | |
| 888 | void __exit cdebug_exit(void) |
| 889 | { |
| 890 | } |
| 891 | |
| 892 | #endif |
| 893 | |
Karsten Keil | 17f0cd2 | 2007-02-28 20:13:50 -0800 | [diff] [blame] | 894 | EXPORT_SYMBOL(cdebbuf_free); |
Linus Torvalds | 1da177e | 2005-04-16 15:20:36 -0700 | [diff] [blame] | 895 | EXPORT_SYMBOL(capi_cmsg2message); |
| 896 | EXPORT_SYMBOL(capi_message2cmsg); |
| 897 | EXPORT_SYMBOL(capi_cmsg_header); |
| 898 | EXPORT_SYMBOL(capi_cmd2str); |
| 899 | EXPORT_SYMBOL(capi_cmsg2str); |
| 900 | EXPORT_SYMBOL(capi_message2str); |