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 static int irda_param_unpack(__u8 *buf, char *fmt, ...);
  55
  56 /* Parameter value call table. Must match PV_TYPE */
  57 static 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
  67 static 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  */
  80 static 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  */
 107 static 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  */
 128 static 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 lenght 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 lenght 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", __FUNCTION__);
 152                         p.pl = 1;
 153                 } else if (p.pv.i < 0xffff) {
 154                         IRDA_DEBUG(2, "%s(), using 2 bytes\n", __FUNCTION__);
 155                         p.pl = 2;
 156                 } else {
 157                         IRDA_DEBUG(2, "%s(), using 4 bytes\n", __FUNCTION__);
 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 to short for insertion!\n",
 164                              __FUNCTION__);
 165                 return -1;
 166         }
 167         IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d, pi=%d\n", __FUNCTION__,
 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                              __FUNCTION__, 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  */
 204 static 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 lenght we extract */
 210         int err;
 211
 212         p.pi = pi;     /* In case handler needs to know */
 213         p.pl = buf[1]; /* Extract lenght 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 to short for parsing! "
 220                              "Need %d bytes, but len is only %d\n",
 221                              __FUNCTION__, 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                            __FUNCTION__, 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                              __FUNCTION__, 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", __FUNCTION__,
 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  */
 290 static 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", __FUNCTION__);
 298
 299         p.pi = pi;     /* In case handler needs to know */
 300         p.pl = buf[1]; /* Extract lenght of value */
 301
 302         IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d\n", __FUNCTION__,
 303                    p.pi, p.pl);
 304
 305         /* Check if buffer is long enough for parsing */
 306         if (len < (2+p.pl)) {
 307                 IRDA_WARNING("%s: buffer to short for parsing! "
 308                              "Need %d bytes, but len is only %d\n",
 309                              __FUNCTION__, p.pl, len);
 310                 return -1;
 311         }
 312
 313         /* Should be safe to copy string like this since we have already
 314          * checked that the buffer is long enough */
 315         strncpy(str, buf+2, p.pl);
 316
 317         IRDA_DEBUG(2, "%s(), str=0x%02x 0x%02x\n", __FUNCTION__,
 318                    (__u8) str[0], (__u8) str[1]);
 319
 320         /* Null terminate string */
 321         str[p.pl+1] = '\0';
 322
 323         p.pv.c = str; /* Handler will need to take a copy */
 324
 325         /* Call handler for this parameter */
 326         err = (*func)(self, &p, PV_PUT);
 327         if (err < 0)
 328                 return err;
 329
 330         return p.pl+2; /* Extracted pl+2 bytes */
 331 }
 332
 333 /*
 334  * Function irda_extract_octseq (self, buf, len, type, func)
 335  */
 336 static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
 337                                PV_TYPE type, PI_HANDLER func)
 338 {
 339         irda_param_t p;
 340
 341         p.pi = pi;     /* In case handler needs to know */
 342         p.pl = buf[1]; /* Extract lenght of value */
 343
 344         /* Check if buffer is long enough for parsing */
 345         if (len < (2+p.pl)) {
 346                 IRDA_WARNING("%s: buffer to short for parsing! "
 347                              "Need %d bytes, but len is only %d\n",
 348                              __FUNCTION__, p.pl, len);
 349                 return -1;
 350         }
 351
 352         IRDA_DEBUG(0, "%s(), not impl\n", __FUNCTION__);
 353
 354         return p.pl+2; /* Extracted pl+2 bytes */
 355 }
 356
 357 /*
 358  * Function irda_param_pack (skb, fmt, ...)
 359  *
 360  *    Format:
 361  *        'i' = 32 bits integer
 362  *        's' = string
 363  *
 364  */
 365 int irda_param_pack(__u8 *buf, char *fmt, ...)
 366 {
 367         irda_pv_t arg;
 368         va_list args;
 369         char *p;
 370         int n = 0;
 371
 372         va_start(args, fmt);
 373
 374         for (p = fmt; *p != '\0'; p++) {
 375                 switch (*p) {
 376                 case 'b':  /* 8 bits unsigned byte */
 377                         buf[n++] = (__u8)va_arg(args, int);
 378                         break;
 379                 case 's':  /* 16 bits unsigned short */
 380                         arg.i = (__u16)va_arg(args, int);
 381                         put_unaligned((__u16)arg.i, (__u16 *)(buf+n)); n+=2;
 382                         break;
 383                 case 'i':  /* 32 bits unsigned integer */
 384                         arg.i = va_arg(args, __u32);
 385                         put_unaligned(arg.i, (__u32 *)(buf+n)); n+=4;
 386                         break;
 387 #if 0
 388                 case 'c': /* \0 terminated string */
 389                         arg.c = va_arg(args, char *);
 390                         strcpy(buf+n, arg.c);
 391                         n += strlen(arg.c) + 1;
 392                         break;
 393 #endif
 394                 default:
 395                         va_end(args);
 396                         return -1;
 397                 }
 398         }
 399         va_end(args);
 400
 401         return 0;
 402 }
 403 EXPORT_SYMBOL(irda_param_pack);
 404
 405 /*
 406  * Function irda_param_unpack (skb, fmt, ...)
 407  */
 408 static int irda_param_unpack(__u8 *buf, char *fmt, ...)
 409 {
 410         irda_pv_t arg;
 411         va_list args;
 412         char *p;
 413         int n = 0;
 414
 415         va_start(args, fmt);
 416
 417         for (p = fmt; *p != '\0'; p++) {
 418                 switch (*p) {
 419                 case 'b':  /* 8 bits byte */
 420                         arg.ip = va_arg(args, __u32 *);
 421                         *arg.ip = buf[n++];
 422                         break;
 423                 case 's':  /* 16 bits short */
 424                         arg.ip = va_arg(args, __u32 *);
 425                         *arg.ip = get_unaligned((__u16 *)(buf+n)); n+=2;
 426                         break;
 427                 case 'i':  /* 32 bits unsigned integer */
 428                         arg.ip = va_arg(args, __u32 *);
 429                         *arg.ip = get_unaligned((__u32 *)(buf+n)); n+=4;
 430                         break;
 431 #if 0
 432                 case 'c':   /* \0 terminated string */
 433                         arg.c = va_arg(args, char *);
 434                         strcpy(arg.c, buf+n);
 435                         n += strlen(arg.c) + 1;
 436                         break;
 437 #endif
 438                 default:
 439                         va_end(args);
 440                         return -1;
 441                 }
 442
 443         }
 444         va_end(args);
 445
 446         return 0;
 447 }
 448
 449 /*
 450  * Function irda_param_insert (self, pi, buf, len, info)
 451  *
 452  *    Insert the specified parameter (pi) into buffer. Returns number of
 453  *    bytes inserted
 454  */
 455 int irda_param_insert(void *self, __u8 pi, __u8 *buf, int len,
 456                       pi_param_info_t *info)
 457 {
 458         pi_minor_info_t *pi_minor_info;
 459         __u8 pi_minor;
 460         __u8 pi_major;
 461         int type;
 462         int ret = -1;
 463         int n = 0;
 464
 465         IRDA_ASSERT(buf != NULL, return ret;);
 466         IRDA_ASSERT(info != 0, return ret;);
 467
 468         pi_minor = pi & info->pi_mask;
 469         pi_major = pi >> info->pi_major_offset;
 470
 471         /* Check if the identifier value (pi) is valid */
 472         if ((pi_major > info->len-1) ||
 473             (pi_minor > info->tables[pi_major].len-1))
 474         {
 475                 IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n",
 476                            __FUNCTION__, pi);
 477
 478                 /* Skip this parameter */
 479                 return -1;
 480         }
 481
 482         /* Lookup the info on how to parse this parameter */
 483         pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
 484
 485         /* Find expected data type for this parameter identifier (pi)*/
 486         type = pi_minor_info->type;
 487
 488         /*  Check if handler has been implemented */
 489         if (!pi_minor_info->func) {
 490                 IRDA_MESSAGE("%s: no handler for pi=%#x\n", __FUNCTION__, pi);
 491                 /* Skip this parameter */
 492                 return -1;
 493         }
 494
 495         /* Insert parameter value */
 496         ret = (*pv_insert_table[type & PV_MASK])(self, buf+n, len, pi, type,
 497                                                  pi_minor_info->func);
 498         return ret;
 499 }
 500 EXPORT_SYMBOL(irda_param_insert);
 501
 502 /*
 503  * Function irda_param_extract (self, buf, len, info)
 504  *
 505  *    Parse all parameters. If len is correct, then everything should be
 506  *    safe. Returns the number of bytes that was parsed
 507  *
 508  */
 509 static int irda_param_extract(void *self, __u8 *buf, int len,
 510                               pi_param_info_t *info)
 511 {
 512         pi_minor_info_t *pi_minor_info;
 513         __u8 pi_minor;
 514         __u8 pi_major;
 515         int type;
 516         int ret = -1;
 517         int n = 0;
 518
 519         IRDA_ASSERT(buf != NULL, return ret;);
 520         IRDA_ASSERT(info != 0, return ret;);
 521
 522         pi_minor = buf[n] & info->pi_mask;
 523         pi_major = buf[n] >> info->pi_major_offset;
 524
 525         /* Check if the identifier value (pi) is valid */
 526         if ((pi_major > info->len-1) ||
 527             (pi_minor > info->tables[pi_major].len-1))
 528         {
 529                 IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n",
 530                            __FUNCTION__, buf[0]);
 531
 532                 /* Skip this parameter */
 533                 return 2 + buf[n + 1];  /* Continue */
 534         }
 535
 536         /* Lookup the info on how to parse this parameter */
 537         pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
 538
 539         /* Find expected data type for this parameter identifier (pi)*/
 540         type = pi_minor_info->type;
 541
 542         IRDA_DEBUG(3, "%s(), pi=[%d,%d], type=%d\n", __FUNCTION__,
 543                    pi_major, pi_minor, type);
 544
 545         /*  Check if handler has been implemented */
 546         if (!pi_minor_info->func) {
 547                 IRDA_MESSAGE("%s: no handler for pi=%#x\n",
 548                              __FUNCTION__, buf[n]);
 549                 /* Skip this parameter */
 550                 return 2 + buf[n + 1]; /* Continue */
 551         }
 552
 553         /* Parse parameter value */
 554         ret = (*pv_extract_table[type & PV_MASK])(self, buf+n, len, buf[n],
 555                                                   type, pi_minor_info->func);
 556         return ret;
 557 }
 558
 559 /*
 560  * Function irda_param_extract_all (self, buf, len, info)
 561  *
 562  *    Parse all parameters. If len is correct, then everything should be
 563  *    safe. Returns the number of bytes that was parsed
 564  *
 565  */
 566 int irda_param_extract_all(void *self, __u8 *buf, int len,
 567                            pi_param_info_t *info)
 568 {
 569         int ret = -1;
 570         int n = 0;
 571
 572         IRDA_ASSERT(buf != NULL, return ret;);
 573         IRDA_ASSERT(info != 0, return ret;);
 574
 575         /*
 576          * Parse all parameters. Each parameter must be at least two bytes
 577          * long or else there is no point in trying to parse it
 578          */
 579         while (len > 2) {
 580                 ret = irda_param_extract(self, buf+n, len, info);
 581                 if (ret < 0)
 582                         return ret;
 583
 584                 n += ret;
 585                 len -= ret;
 586         }
 587         return n;
 588 }
 589 EXPORT_SYMBOL(irda_param_extract_all);