1/********************************************************************* 2 * 3 * Filename: parameters.c 4 * Version: 1.0 5 * Description: A more general way to handle (pi,pl,pv) parameters 6 * Status: Experimental. 7 * Author: Dag Brattli <dagb@cs.uit.no> 8 * Created at: Mon Jun 7 10:25:11 1999 9 * Modified at: Sun Jan 30 14:08:39 2000 10 * Modified by: Dag Brattli <dagb@cs.uit.no> 11 * 12 * Copyright (c) 1999-2000 Dag Brattli, All Rights Reserved. 13 * 14 * This program is free software; you can redistribute it and/or 15 * modify it under the terms of the GNU General Public License as 16 * published by the Free Software Foundation; either version 2 of 17 * the License, or (at your option) any later version. 18 * 19 * This program is distributed in the hope that it will be useful, 20 * but WITHOUT ANY WARRANTY; without even the implied warranty of 21 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 22 * GNU General Public License for more details. 23 * 24 * You should have received a copy of the GNU General Public License 25 * along with this program; if not, see <http://www.gnu.org/licenses/>. 26 * 27 ********************************************************************/ 28 29#include <linux/types.h> 30#include <linux/module.h> 31 32#include <asm/unaligned.h> 33#include <asm/byteorder.h> 34 35#include <net/irda/irda.h> 36#include <net/irda/parameters.h> 37 38static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi, 39 PV_TYPE type, PI_HANDLER func); 40static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi, 41 PV_TYPE type, PI_HANDLER func); 42static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi, 43 PV_TYPE type, PI_HANDLER func); 44static int irda_extract_no_value(void *self, __u8 *buf, int len, __u8 pi, 45 PV_TYPE type, PI_HANDLER func); 46 47static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi, 48 PV_TYPE type, PI_HANDLER func); 49static int irda_insert_no_value(void *self, __u8 *buf, int len, __u8 pi, 50 PV_TYPE type, PI_HANDLER func); 51 52static int irda_param_unpack(__u8 *buf, char *fmt, ...); 53 54/* Parameter value call table. Must match PV_TYPE */ 55static const PV_HANDLER pv_extract_table[] = { 56 irda_extract_integer, /* Handler for any length integers */ 57 irda_extract_integer, /* Handler for 8 bits integers */ 58 irda_extract_integer, /* Handler for 16 bits integers */ 59 irda_extract_string, /* Handler for strings */ 60 irda_extract_integer, /* Handler for 32 bits integers */ 61 irda_extract_octseq, /* Handler for octet sequences */ 62 irda_extract_no_value /* Handler for no value parameters */ 63}; 64 65static const PV_HANDLER pv_insert_table[] = { 66 irda_insert_integer, /* Handler for any length integers */ 67 irda_insert_integer, /* Handler for 8 bits integers */ 68 irda_insert_integer, /* Handler for 16 bits integers */ 69 NULL, /* Handler for strings */ 70 irda_insert_integer, /* Handler for 32 bits integers */ 71 NULL, /* Handler for octet sequences */ 72 irda_insert_no_value /* Handler for no value parameters */ 73}; 74 75/* 76 * Function irda_insert_no_value (self, buf, len, pi, type, func) 77 */ 78static int irda_insert_no_value(void *self, __u8 *buf, int len, __u8 pi, 79 PV_TYPE type, PI_HANDLER func) 80{ 81 irda_param_t p; 82 int ret; 83 84 p.pi = pi; 85 p.pl = 0; 86 87 /* Call handler for this parameter */ 88 ret = (*func)(self, &p, PV_GET); 89 90 /* Extract values anyway, since handler may need them */ 91 irda_param_pack(buf, "bb", p.pi, p.pl); 92 93 if (ret < 0) 94 return ret; 95 96 return 2; /* Inserted pl+2 bytes */ 97} 98 99/* 100 * Function irda_extract_no_value (self, buf, len, type, func) 101 * 102 * Extracts a parameter without a pv field (pl=0) 103 * 104 */ 105static int irda_extract_no_value(void *self, __u8 *buf, int len, __u8 pi, 106 PV_TYPE type, PI_HANDLER func) 107{ 108 irda_param_t p; 109 int ret; 110 111 /* Extract values anyway, since handler may need them */ 112 irda_param_unpack(buf, "bb", &p.pi, &p.pl); 113 114 /* Call handler for this parameter */ 115 ret = (*func)(self, &p, PV_PUT); 116 117 if (ret < 0) 118 return ret; 119 120 return 2; /* Extracted pl+2 bytes */ 121} 122 123/* 124 * Function irda_insert_integer (self, buf, len, pi, type, func) 125 */ 126static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi, 127 PV_TYPE type, PI_HANDLER func) 128{ 129 irda_param_t p; 130 int n = 0; 131 int err; 132 133 p.pi = pi; /* In case handler needs to know */ 134 p.pl = type & PV_MASK; /* The integer type codes the length as well */ 135 p.pv.i = 0; /* Clear value */ 136 137 /* Call handler for this parameter */ 138 err = (*func)(self, &p, PV_GET); 139 if (err < 0) 140 return err; 141 142 /* 143 * If parameter length is still 0, then (1) this is an any length 144 * integer, and (2) the handler function does not care which length 145 * we choose to use, so we pick the one the gives the fewest bytes. 146 */ 147 if (p.pl == 0) { 148 if (p.pv.i < 0xff) { 149 pr_debug("%s(), using 1 byte\n", __func__); 150 p.pl = 1; 151 } else if (p.pv.i < 0xffff) { 152 pr_debug("%s(), using 2 bytes\n", __func__); 153 p.pl = 2; 154 } else { 155 pr_debug("%s(), using 4 bytes\n", __func__); 156 p.pl = 4; /* Default length */ 157 } 158 } 159 /* Check if buffer is long enough for insertion */ 160 if (len < (2+p.pl)) { 161 net_warn_ratelimited("%s: buffer too short for insertion!\n", 162 __func__); 163 return -1; 164 } 165 pr_debug("%s(), pi=%#x, pl=%d, pi=%d\n", __func__, 166 p.pi, p.pl, p.pv.i); 167 switch (p.pl) { 168 case 1: 169 n += irda_param_pack(buf, "bbb", p.pi, p.pl, (__u8) p.pv.i); 170 break; 171 case 2: 172 if (type & PV_BIG_ENDIAN) 173 p.pv.i = cpu_to_be16((__u16) p.pv.i); 174 else 175 p.pv.i = cpu_to_le16((__u16) p.pv.i); 176 n += irda_param_pack(buf, "bbs", p.pi, p.pl, (__u16) p.pv.i); 177 break; 178 case 4: 179 if (type & PV_BIG_ENDIAN) 180 cpu_to_be32s(&p.pv.i); 181 else 182 cpu_to_le32s(&p.pv.i); 183 n += irda_param_pack(buf, "bbi", p.pi, p.pl, p.pv.i); 184 185 break; 186 default: 187 net_warn_ratelimited("%s: length %d not supported\n", 188 __func__, p.pl); 189 /* Skip parameter */ 190 return -1; 191 } 192 193 return p.pl+2; /* Inserted pl+2 bytes */ 194} 195 196/* 197 * Function irda_extract integer (self, buf, len, pi, type, func) 198 * 199 * Extract a possibly variable length integer from buffer, and call 200 * handler for processing of the parameter 201 */ 202static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi, 203 PV_TYPE type, PI_HANDLER func) 204{ 205 irda_param_t p; 206 int n = 0; 207 int extract_len; /* Real length we extract */ 208 int err; 209 210 p.pi = pi; /* In case handler needs to know */ 211 p.pl = buf[1]; /* Extract length of value */ 212 p.pv.i = 0; /* Clear value */ 213 extract_len = p.pl; /* Default : extract all */ 214 215 /* Check if buffer is long enough for parsing */ 216 if (len < (2+p.pl)) { 217 net_warn_ratelimited("%s: buffer too short for parsing! Need %d bytes, but len is only %d\n", 218 __func__, p.pl, len); 219 return -1; 220 } 221 222 /* 223 * Check that the integer length is what we expect it to be. If the 224 * handler want a 16 bits integer then a 32 bits is not good enough 225 * PV_INTEGER means that the handler is flexible. 226 */ 227 if (((type & PV_MASK) != PV_INTEGER) && ((type & PV_MASK) != p.pl)) { 228 net_err_ratelimited("%s: invalid parameter length! Expected %d bytes, but value had %d bytes!\n", 229 __func__, type & PV_MASK, p.pl); 230 231 /* Most parameters are bit/byte fields or little endian, 232 * so it's ok to only extract a subset of it (the subset 233 * that the handler expect). This is necessary, as some 234 * broken implementations seems to add extra undefined bits. 235 * If the parameter is shorter than we expect or is big 236 * endian, we can't play those tricks. Jean II */ 237 if((p.pl < (type & PV_MASK)) || (type & PV_BIG_ENDIAN)) { 238 /* Skip parameter */ 239 return p.pl+2; 240 } else { 241 /* Extract subset of it, fallthrough */ 242 extract_len = type & PV_MASK; 243 } 244 } 245 246 247 switch (extract_len) { 248 case 1: 249 n += irda_param_unpack(buf+2, "b", &p.pv.i); 250 break; 251 case 2: 252 n += irda_param_unpack(buf+2, "s", &p.pv.i); 253 if (type & PV_BIG_ENDIAN) 254 p.pv.i = be16_to_cpu((__u16) p.pv.i); 255 else 256 p.pv.i = le16_to_cpu((__u16) p.pv.i); 257 break; 258 case 4: 259 n += irda_param_unpack(buf+2, "i", &p.pv.i); 260 if (type & PV_BIG_ENDIAN) 261 be32_to_cpus(&p.pv.i); 262 else 263 le32_to_cpus(&p.pv.i); 264 break; 265 default: 266 net_warn_ratelimited("%s: length %d not supported\n", 267 __func__, p.pl); 268 269 /* Skip parameter */ 270 return p.pl+2; 271 } 272 273 pr_debug("%s(), pi=%#x, pl=%d, pi=%d\n", __func__, 274 p.pi, p.pl, p.pv.i); 275 /* Call handler for this parameter */ 276 err = (*func)(self, &p, PV_PUT); 277 if (err < 0) 278 return err; 279 280 return p.pl+2; /* Extracted pl+2 bytes */ 281} 282 283/* 284 * Function irda_extract_string (self, buf, len, type, func) 285 */ 286static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi, 287 PV_TYPE type, PI_HANDLER func) 288{ 289 char str[33]; 290 irda_param_t p; 291 int err; 292 293 p.pi = pi; /* In case handler needs to know */ 294 p.pl = buf[1]; /* Extract length of value */ 295 if (p.pl > 32) 296 p.pl = 32; 297 298 pr_debug("%s(), pi=%#x, pl=%d\n", __func__, 299 p.pi, p.pl); 300 301 /* Check if buffer is long enough for parsing */ 302 if (len < (2+p.pl)) { 303 net_warn_ratelimited("%s: buffer too short for parsing! Need %d bytes, but len is only %d\n", 304 __func__, p.pl, len); 305 return -1; 306 } 307 308 /* Should be safe to copy string like this since we have already 309 * checked that the buffer is long enough */ 310 strncpy(str, buf+2, p.pl); 311 312 pr_debug("%s(), str=0x%02x 0x%02x\n", 313 __func__, (__u8)str[0], (__u8)str[1]); 314 315 /* Null terminate string */ 316 str[p.pl] = '\0'; 317 318 p.pv.c = str; /* Handler will need to take a copy */ 319 320 /* Call handler for this parameter */ 321 err = (*func)(self, &p, PV_PUT); 322 if (err < 0) 323 return err; 324 325 return p.pl+2; /* Extracted pl+2 bytes */ 326} 327 328/* 329 * Function irda_extract_octseq (self, buf, len, type, func) 330 */ 331static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi, 332 PV_TYPE type, PI_HANDLER func) 333{ 334 irda_param_t p; 335 336 p.pi = pi; /* In case handler needs to know */ 337 p.pl = buf[1]; /* Extract length of value */ 338 339 /* Check if buffer is long enough for parsing */ 340 if (len < (2+p.pl)) { 341 net_warn_ratelimited("%s: buffer too short for parsing! Need %d bytes, but len is only %d\n", 342 __func__, p.pl, len); 343 return -1; 344 } 345 346 pr_debug("%s(), not impl\n", __func__); 347 348 return p.pl+2; /* Extracted pl+2 bytes */ 349} 350 351/* 352 * Function irda_param_pack (skb, fmt, ...) 353 * 354 * Format: 355 * 'i' = 32 bits integer 356 * 's' = string 357 * 358 */ 359int irda_param_pack(__u8 *buf, char *fmt, ...) 360{ 361 irda_pv_t arg; 362 va_list args; 363 char *p; 364 int n = 0; 365 366 va_start(args, fmt); 367 368 for (p = fmt; *p != '\0'; p++) { 369 switch (*p) { 370 case 'b': /* 8 bits unsigned byte */ 371 buf[n++] = (__u8)va_arg(args, int); 372 break; 373 case 's': /* 16 bits unsigned short */ 374 arg.i = (__u16)va_arg(args, int); 375 put_unaligned((__u16)arg.i, (__u16 *)(buf+n)); n+=2; 376 break; 377 case 'i': /* 32 bits unsigned integer */ 378 arg.i = va_arg(args, __u32); 379 put_unaligned(arg.i, (__u32 *)(buf+n)); n+=4; 380 break; 381#if 0 382 case 'c': /* \0 terminated string */ 383 arg.c = va_arg(args, char *); 384 strcpy(buf+n, arg.c); 385 n += strlen(arg.c) + 1; 386 break; 387#endif 388 default: 389 va_end(args); 390 return -1; 391 } 392 } 393 va_end(args); 394 395 return 0; 396} 397EXPORT_SYMBOL(irda_param_pack); 398 399/* 400 * Function irda_param_unpack (skb, fmt, ...) 401 */ 402static int irda_param_unpack(__u8 *buf, char *fmt, ...) 403{ 404 irda_pv_t arg; 405 va_list args; 406 char *p; 407 int n = 0; 408 409 va_start(args, fmt); 410 411 for (p = fmt; *p != '\0'; p++) { 412 switch (*p) { 413 case 'b': /* 8 bits byte */ 414 arg.ip = va_arg(args, __u32 *); 415 *arg.ip = buf[n++]; 416 break; 417 case 's': /* 16 bits short */ 418 arg.ip = va_arg(args, __u32 *); 419 *arg.ip = get_unaligned((__u16 *)(buf+n)); n+=2; 420 break; 421 case 'i': /* 32 bits unsigned integer */ 422 arg.ip = va_arg(args, __u32 *); 423 *arg.ip = get_unaligned((__u32 *)(buf+n)); n+=4; 424 break; 425#if 0 426 case 'c': /* \0 terminated string */ 427 arg.c = va_arg(args, char *); 428 strcpy(arg.c, buf+n); 429 n += strlen(arg.c) + 1; 430 break; 431#endif 432 default: 433 va_end(args); 434 return -1; 435 } 436 437 } 438 va_end(args); 439 440 return 0; 441} 442 443/* 444 * Function irda_param_insert (self, pi, buf, len, info) 445 * 446 * Insert the specified parameter (pi) into buffer. Returns number of 447 * bytes inserted 448 */ 449int irda_param_insert(void *self, __u8 pi, __u8 *buf, int len, 450 pi_param_info_t *info) 451{ 452 const pi_minor_info_t *pi_minor_info; 453 __u8 pi_minor; 454 __u8 pi_major; 455 int type; 456 int ret = -1; 457 int n = 0; 458 459 IRDA_ASSERT(buf != NULL, return ret;); 460 IRDA_ASSERT(info != NULL, return ret;); 461 462 pi_minor = pi & info->pi_mask; 463 pi_major = pi >> info->pi_major_offset; 464 465 /* Check if the identifier value (pi) is valid */ 466 if ((pi_major > info->len-1) || 467 (pi_minor > info->tables[pi_major].len-1)) 468 { 469 pr_debug("%s(), no handler for parameter=0x%02x\n", 470 __func__, pi); 471 472 /* Skip this parameter */ 473 return -1; 474 } 475 476 /* Lookup the info on how to parse this parameter */ 477 pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor]; 478 479 /* Find expected data type for this parameter identifier (pi)*/ 480 type = pi_minor_info->type; 481 482 /* Check if handler has been implemented */ 483 if (!pi_minor_info->func) { 484 net_info_ratelimited("%s: no handler for pi=%#x\n", 485 __func__, pi); 486 /* Skip this parameter */ 487 return -1; 488 } 489 490 /* Insert parameter value */ 491 ret = (*pv_insert_table[type & PV_MASK])(self, buf+n, len, pi, type, 492 pi_minor_info->func); 493 return ret; 494} 495EXPORT_SYMBOL(irda_param_insert); 496 497/* 498 * Function irda_param_extract (self, buf, len, info) 499 * 500 * Parse all parameters. If len is correct, then everything should be 501 * safe. Returns the number of bytes that was parsed 502 * 503 */ 504static int irda_param_extract(void *self, __u8 *buf, int len, 505 pi_param_info_t *info) 506{ 507 const pi_minor_info_t *pi_minor_info; 508 __u8 pi_minor; 509 __u8 pi_major; 510 int type; 511 int ret = -1; 512 int n = 0; 513 514 IRDA_ASSERT(buf != NULL, return ret;); 515 IRDA_ASSERT(info != NULL, return ret;); 516 517 pi_minor = buf[n] & info->pi_mask; 518 pi_major = buf[n] >> info->pi_major_offset; 519 520 /* Check if the identifier value (pi) is valid */ 521 if ((pi_major > info->len-1) || 522 (pi_minor > info->tables[pi_major].len-1)) 523 { 524 pr_debug("%s(), no handler for parameter=0x%02x\n", 525 __func__, buf[0]); 526 527 /* Skip this parameter */ 528 return 2 + buf[n + 1]; /* Continue */ 529 } 530 531 /* Lookup the info on how to parse this parameter */ 532 pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor]; 533 534 /* Find expected data type for this parameter identifier (pi)*/ 535 type = pi_minor_info->type; 536 537 pr_debug("%s(), pi=[%d,%d], type=%d\n", __func__, 538 pi_major, pi_minor, type); 539 540 /* Check if handler has been implemented */ 541 if (!pi_minor_info->func) { 542 net_info_ratelimited("%s: no handler for pi=%#x\n", 543 __func__, buf[n]); 544 /* Skip this parameter */ 545 return 2 + buf[n + 1]; /* Continue */ 546 } 547 548 /* Parse parameter value */ 549 ret = (*pv_extract_table[type & PV_MASK])(self, buf+n, len, buf[n], 550 type, pi_minor_info->func); 551 return ret; 552} 553 554/* 555 * Function irda_param_extract_all (self, buf, len, info) 556 * 557 * Parse all parameters. If len is correct, then everything should be 558 * safe. Returns the number of bytes that was parsed 559 * 560 */ 561int irda_param_extract_all(void *self, __u8 *buf, int len, 562 pi_param_info_t *info) 563{ 564 int ret = -1; 565 int n = 0; 566 567 IRDA_ASSERT(buf != NULL, return ret;); 568 IRDA_ASSERT(info != NULL, return ret;); 569 570 /* 571 * Parse all parameters. Each parameter must be at least two bytes 572 * long or else there is no point in trying to parse it 573 */ 574 while (len > 2) { 575 ret = irda_param_extract(self, buf+n, len, info); 576 if (ret < 0) 577 return ret; 578 579 n += ret; 580 len -= ret; 581 } 582 return n; 583} 584EXPORT_SYMBOL(irda_param_extract_all); 585