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