net/irda/parameters.c

   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
  40 static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi,
  41                                 PV_TYPE type, PI_HANDLER func);
  42 static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi,
  43                                PV_TYPE type, PI_HANDLER func);
  44 static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
  45                                PV_TYPE type, PI_HANDLER func);
  46 static int irda_extract_no_value(void *self, __u8 *buf, int len, __u8 pi,
  47                                  PV_TYPE type, PI_HANDLER func);
  48
  49 static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi,
  50                                PV_TYPE type, PI_HANDLER func);
  51 static int irda_insert_no_value(void *self, __u8 *buf, int len, __u8 pi,
  52                                 PV_TYPE type, PI_HANDLER func);
  53
  54 /* Parameter value call table. Must match PV_TYPE */
  55 static 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
  65 static 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  */
  78 static 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  */
 105 static 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  */
 126 static 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 lenght 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 lenght 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                         IRDA_DEBUG(2, "%s(), using 1 byte\n", __FUNCTION__);
 150                         p.pl = 1;
 151                 } else if (p.pv.i < 0xffff) {
 152                         IRDA_DEBUG(2, "%s(), using 2 bytes\n", __FUNCTION__);
 153                         p.pl = 2;
 154                 } else {
 155                         IRDA_DEBUG(2, "%s(), using 4 bytes\n", __FUNCTION__);
 156                         p.pl = 4; /* Default length */
 157                 }
 158         }
 159         /* Check if buffer is long enough for insertion */
 160         if (len < (2+p.pl)) {
 161                 WARNING("%s: buffer to short for insertion!\n", __FUNCTION__);
 162                 return -1;
 163         }
 164         IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d, pi=%d\n", __FUNCTION__,
 165                    p.pi, p.pl, p.pv.i);
 166         switch (p.pl) {
 167         case 1:
 168                 n += irda_param_pack(buf, "bbb", p.pi, p.pl, (__u8) p.pv.i);
 169                 break;
 170         case 2:
 171                 if (type & PV_BIG_ENDIAN)
 172                         p.pv.i = cpu_to_be16((__u16) p.pv.i);
 173                 else
 174                         p.pv.i = cpu_to_le16((__u16) p.pv.i);
 175                 n += irda_param_pack(buf, "bbs", p.pi, p.pl, (__u16) p.pv.i);
 176                 break;
 177         case 4:
 178                 if (type & PV_BIG_ENDIAN)
 179                         cpu_to_be32s(&p.pv.i);
 180                 else
 181                         cpu_to_le32s(&p.pv.i);
 182                 n += irda_param_pack(buf, "bbi", p.pi, p.pl, p.pv.i);
 183
 184                 break;
 185         default:
 186                 WARNING("%s: length %d not supported\n", __FUNCTION__, p.pl);
 187                 /* Skip parameter */
 188                 return -1;
 189         }
 190
 191         return p.pl+2; /* Inserted pl+2 bytes */
 192 }
 193
 194 /*
 195  * Function irda_extract integer (self, buf, len, pi, type, func)
 196  *
 197  *    Extract a possibly variable length integer from buffer, and call
 198  *    handler for processing of the parameter
 199  */
 200 static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi,
 201                                 PV_TYPE type, PI_HANDLER func)
 202 {
 203         irda_param_t p;
 204         int n = 0;
 205         int extract_len;        /* Real lenght we extract */
 206         int err;
 207
 208         p.pi = pi;     /* In case handler needs to know */
 209         p.pl = buf[1]; /* Extract lenght of value */
 210         p.pv.i = 0;    /* Clear value */
 211         extract_len = p.pl;     /* Default : extract all */
 212
 213         /* Check if buffer is long enough for parsing */
 214         if (len < (2+p.pl)) {
 215                 WARNING("%s: buffer to short for parsing! "
 216                         "Need %d bytes, but len is only %d\n",
 217                         __FUNCTION__, p.pl, len);
 218                 return -1;
 219         }
 220
 221         /*
 222          * Check that the integer length is what we expect it to be. If the
 223          * handler want a 16 bits integer then a 32 bits is not good enough
 224          * PV_INTEGER means that the handler is flexible.
 225          */
 226         if (((type & PV_MASK) != PV_INTEGER) && ((type & PV_MASK) != p.pl)) {
 227                 ERROR("%s: invalid parameter length! "
 228                       "Expected %d bytes, but value had %d bytes!\n",
 229                       __FUNCTION__, 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                 WARNING("%s: length %d not supported\n", __FUNCTION__, p.pl);
 267
 268                 /* Skip parameter */
 269                 return p.pl+2;
 270         }
 271
 272         IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d, pi=%d\n", __FUNCTION__,
 273                    p.pi, p.pl, p.pv.i);
 274         /* Call handler for this parameter */
 275         err = (*func)(self, &p, PV_PUT);
 276         if (err < 0)
 277                 return err;
 278
 279         return p.pl+2; /* Extracted pl+2 bytes */
 280 }
 281
 282 /*
 283  * Function irda_extract_string (self, buf, len, type, func)
 284  */
 285 static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi,
 286                                PV_TYPE type, PI_HANDLER func)
 287 {
 288         char str[33];
 289         irda_param_t p;
 290         int err;
 291
 292         IRDA_DEBUG(2, "%s()\n", __FUNCTION__);
 293
 294         p.pi = pi;     /* In case handler needs to know */
 295         p.pl = buf[1]; /* Extract lenght of value */
 296
 297         IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d\n", __FUNCTION__,
 298                    p.pi, p.pl);
 299
 300         /* Check if buffer is long enough for parsing */
 301         if (len < (2+p.pl)) {
 302                 WARNING("%s: buffer to short for parsing! "
 303                         "Need %d bytes, but len is only %d\n",
 304                         __FUNCTION__, 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         IRDA_DEBUG(2, "%s(), str=0x%02x 0x%02x\n", __FUNCTION__,
 313                    (__u8) str[0], (__u8) str[1]);
 314
 315         /* Null terminate string */
 316         str[p.pl+1] = '\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  */
 331 static 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 lenght of value */
 338
 339         /* Check if buffer is long enough for parsing */
 340         if (len < (2+p.pl)) {
 341                 WARNING("%s: buffer to short for parsing! "
 342                         "Need %d bytes, but len is only %d\n",
 343                         __FUNCTION__, p.pl, len);
 344                 return -1;
 345         }
 346
 347         IRDA_DEBUG(0, "%s(), not impl\n", __FUNCTION__);
 348
 349         return p.pl+2; /* Extracted pl+2 bytes */
 350 }
 351
 352 /*
 353  * Function irda_param_pack (skb, fmt, ...)
 354  *
 355  *    Format:
 356  *        'i' = 32 bits integer
 357  *        's' = string
 358  *
 359  */
 360 int irda_param_pack(__u8 *buf, char *fmt, ...)
 361 {
 362         irda_pv_t arg;
 363         va_list args;
 364         char *p;
 365         int n = 0;
 366
 367         va_start(args, fmt);
 368
 369         for (p = fmt; *p != '\0'; p++) {
 370                 switch (*p) {
 371                 case 'b':  /* 8 bits unsigned byte */
 372                         buf[n++] = (__u8)va_arg(args, int);
 373                         break;
 374                 case 's':  /* 16 bits unsigned short */
 375                         arg.i = (__u16)va_arg(args, int);
 376                         put_unaligned((__u16)arg.i, (__u16 *)(buf+n)); n+=2;
 377                         break;
 378                 case 'i':  /* 32 bits unsigned integer */
 379                         arg.i = va_arg(args, __u32);
 380                         put_unaligned(arg.i, (__u32 *)(buf+n)); n+=4;
 381                         break;
 382 #if 0
 383                 case 'c': /* \0 terminated string */
 384                         arg.c = va_arg(args, char *);
 385                         strcpy(buf+n, arg.c);
 386                         n += strlen(arg.c) + 1;
 387                         break;
 388 #endif
 389                 default:
 390                         va_end(args);
 391                         return -1;
 392                 }
 393         }
 394         va_end(args);
 395
 396         return 0;
 397 }
 398 EXPORT_SYMBOL(irda_param_pack);
 399
 400 /*
 401  * Function irda_param_unpack (skb, fmt, ...)
 402  */
 403 int irda_param_unpack(__u8 *buf, char *fmt, ...)
 404 {
 405         irda_pv_t arg;
 406         va_list args;
 407         char *p;
 408         int n = 0;
 409
 410         va_start(args, fmt);
 411
 412         for (p = fmt; *p != '\0'; p++) {
 413                 switch (*p) {
 414                 case 'b':  /* 8 bits byte */
 415                         arg.ip = va_arg(args, __u32 *);
 416                         *arg.ip = buf[n++];
 417                         break;
 418                 case 's':  /* 16 bits short */
 419                         arg.ip = va_arg(args, __u32 *);
 420                         *arg.ip = get_unaligned((__u16 *)(buf+n)); n+=2;
 421                         break;
 422                 case 'i':  /* 32 bits unsigned integer */
 423                         arg.ip = va_arg(args, __u32 *);
 424                         *arg.ip = get_unaligned((__u32 *)(buf+n)); n+=4;
 425                         break;
 426 #if 0
 427                 case 'c':   /* \0 terminated string */
 428                         arg.c = va_arg(args, char *);
 429                         strcpy(arg.c, buf+n);
 430                         n += strlen(arg.c) + 1;
 431                         break;
 432 #endif
 433                 default:
 434                         va_end(args);
 435                         return -1;
 436                 }
 437
 438         }
 439         va_end(args);
 440
 441         return 0;
 442 }
 443 EXPORT_SYMBOL(irda_param_unpack);
 444
 445 /*
 446  * Function irda_param_insert (self, pi, buf, len, info)
 447  *
 448  *    Insert the specified parameter (pi) into buffer. Returns number of
 449  *    bytes inserted
 450  */
 451 int irda_param_insert(void *self, __u8 pi, __u8 *buf, int len,
 452                       pi_param_info_t *info)
 453 {
 454         pi_minor_info_t *pi_minor_info;
 455         __u8 pi_minor;
 456         __u8 pi_major;
 457         int type;
 458         int ret = -1;
 459         int n = 0;
 460
 461         ASSERT(buf != NULL, return ret;);
 462         ASSERT(info != 0, return ret;);
 463
 464         pi_minor = pi & info->pi_mask;
 465         pi_major = pi >> info->pi_major_offset;
 466
 467         /* Check if the identifier value (pi) is valid */
 468         if ((pi_major > info->len-1) ||
 469             (pi_minor > info->tables[pi_major].len-1))
 470         {
 471                 IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n",
 472                            __FUNCTION__, pi);
 473
 474                 /* Skip this parameter */
 475                 return -1;
 476         }
 477
 478         /* Lookup the info on how to parse this parameter */
 479         pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
 480
 481         /* Find expected data type for this parameter identifier (pi)*/
 482         type = pi_minor_info->type;
 483
 484         /*  Check if handler has been implemented */
 485         if (!pi_minor_info->func) {
 486                 MESSAGE("%s: no handler for pi=%#x\n", __FUNCTION__, pi);
 487                 /* Skip this parameter */
 488                 return -1;
 489         }
 490
 491         /* Insert parameter value */
 492         ret = (*pv_insert_table[type & PV_MASK])(self, buf+n, len, pi, type,
 493                                                  pi_minor_info->func);
 494         return ret;
 495 }
 496 EXPORT_SYMBOL(irda_param_insert);
 497
 498 /*
 499  * Function irda_param_extract_all (self, buf, len, info)
 500  *
 501  *    Parse all parameters. If len is correct, then everything should be
 502  *    safe. Returns the number of bytes that was parsed
 503  *
 504  */
 505 int irda_param_extract(void *self, __u8 *buf, int len, pi_param_info_t *info)
 506 {
 507         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         ASSERT(buf != NULL, return ret;);
 515         ASSERT(info != 0, 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                 IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n",
 525                            __FUNCTION__, 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         IRDA_DEBUG(3, "%s(), pi=[%d,%d], type=%d\n", __FUNCTION__,
 538                    pi_major, pi_minor, type);
 539
 540         /*  Check if handler has been implemented */
 541         if (!pi_minor_info->func) {
 542                 MESSAGE("%s: no handler for pi=%#x\n", __FUNCTION__, buf[n]);
 543                 /* Skip this parameter */
 544                 return 2 + buf[n + 1]; /* Continue */
 545         }
 546
 547         /* Parse parameter value */
 548         ret = (*pv_extract_table[type & PV_MASK])(self, buf+n, len, buf[n],
 549                                                   type, pi_minor_info->func);
 550         return ret;
 551 }
 552 EXPORT_SYMBOL(irda_param_extract);
 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  */
 561 int 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         ASSERT(buf != NULL, return ret;);
 568         ASSERT(info != 0, 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 }
 584 EXPORT_SYMBOL(irda_param_extract_all);