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:   Tue Oct  5 11:52:54 1999
  10  * Modified by:   Dag Brattli <dagb@cs.uit.no>
  11  *
  12  *     Copyright (c) 1999 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 <asm/unaligned.h>
  32 #include <asm/byteorder.h>
  33
  34 #include <net/irda/irda.h>
  35 #include <net/irda/parameters.h>
  36
  37 static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi,
  38                                 PV_TYPE type, PI_HANDLER func);
  39 static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi,
  40                                PV_TYPE type, PI_HANDLER func);
  41 static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
  42                                PV_TYPE type, PI_HANDLER func);
  43 static int irda_extract_no_value(void *self, __u8 *buf, int len, __u8 pi,
  44                                  PV_TYPE type, PI_HANDLER func);
  45
  46 static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi,
  47                                PV_TYPE type, PI_HANDLER func);
  48 static int irda_insert_no_value(void *self, __u8 *buf, int len, __u8 pi,
  49                                 PV_TYPE type, PI_HANDLER func);
  50
  51 /* Parameter value call table. Must match PV_TYPE */
  52 static PV_HANDLER pv_extract_table[] = {
  53         irda_extract_integer, /* Handler for any length integers */
  54         irda_extract_integer, /* Handler for 8  bits integers */
  55         irda_extract_integer, /* Handler for 16 bits integers */
  56         irda_extract_string,  /* Handler for strings */
  57         irda_extract_integer, /* Handler for 32 bits integers */
  58         irda_extract_octseq,  /* Handler for octet sequences */
  59         irda_extract_no_value /* Handler for no value parameters */
  60 };
  61
  62 static PV_HANDLER pv_insert_table[] = {
  63         irda_insert_integer, /* Handler for any length integers */
  64         irda_insert_integer, /* Handler for 8  bits integers */
  65         irda_insert_integer, /* Handler for 16 bits integers */
  66         NULL,                /* Handler for strings */
  67         irda_insert_integer, /* Handler for 32 bits integers */
  68         NULL,                /* Handler for octet sequences */
  69         irda_insert_no_value /* Handler for no value parameters */
  70 };
  71
  72 /*
  73  * Function irda_insert_no_value (self, buf, len, pi, type, func)
  74  *
  75  *
  76  *
  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         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         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  *
 127  *
 128  */
 129 static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi,
 130                                PV_TYPE type, PI_HANDLER func)
 131 {
 132         param_t p;
 133         int n = 0;
 134         int err;
 135
 136         p.pi = pi;             /* In case handler needs to know */
 137         p.pl = type & PV_MASK; /* The integer type codes the lenght as well */
 138         p.pv.i = 0;            /* Clear value */
 139
 140         /* Call handler for this parameter */
 141         err = (*func)(self, &p, PV_GET);
 142         if (err < 0)
 143                 return err;
 144
 145         /*
 146          * If parameter lenght is still 0, then (1) this is an any length
 147          * integer, and (2) the handler function does not care which length
 148          * we choose to use, so we pick the one the gives the fewest bytes.
 149          */
 150         if (p.pl == 0) {
 151                 if (p.pv.i < 0xff) {
 152                         IRDA_DEBUG(2, __FUNCTION__ "(), using 1 byte\n");
 153                         p.pl = 1;
 154                 } else if (p.pv.i < 0xffff) {
 155                         IRDA_DEBUG(2, __FUNCTION__ "(), using 2 bytes\n");
 156                         p.pl = 2;
 157                 } else {
 158                         IRDA_DEBUG(2, __FUNCTION__ "(), using 4 bytes\n");
 159                         p.pl = 4; /* Default length */
 160                 }
 161         }
 162         /* Check if buffer is long enough for insertion */
 163         if (len < (2+p.pl)) {
 164                 WARNING(__FUNCTION__ "(), buffer to short for insertion!\n");
 165                 return -1;
 166         }
 167         IRDA_DEBUG(2, __FUNCTION__ "(), pi=%#x, pl=%d, pi=%d\n", p.pi, p.pl, p.pv.i);
 168         switch (p.pl) {
 169         case 1:
 170                 n += irda_param_pack(buf, "bbb", p.pi, p.pl, p.pv.b);
 171                 break;
 172         case 2:
 173                 if (type & PV_BIG_ENDIAN)
 174                         cpu_to_be16s(&p.pv.s);
 175                 else
 176                         cpu_to_le16s(&p.pv.s);
 177                 n += irda_param_pack(buf, "bbs", p.pi, p.pl, p.pv.s);
 178                 break;
 179         case 4:
 180                 if (type & PV_BIG_ENDIAN)
 181                         cpu_to_be32s(&p.pv.i);
 182                 else
 183                         cpu_to_le32s(&p.pv.i);
 184                 n += irda_param_pack(buf, "bbi", p.pi, p.pl, p.pv.i);
 185
 186                 break;
 187         default:
 188                 WARNING(__FUNCTION__ "() length %d not supported\n", 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  */
 202 static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi,
 203                                 PV_TYPE type, PI_HANDLER func)
 204 {
 205         param_t p;
 206         int n = 0;
 207         int err;
 208
 209         p.pi = pi;     /* In case handler needs to know */
 210         p.pl = buf[1]; /* Extract lenght of value */
 211         p.pv.i = 0;    /* Clear value */
 212
 213         /* Check if buffer is long enough for parsing */
 214         if (len < (2+p.pl)) {
 215                 WARNING(__FUNCTION__ "(), buffer to short for parsing! "
 216                         "Need %d bytes, but len is only %d\n", p.pl, len);
 217                 return -1;
 218         }
 219
 220         /*
 221          * Check that the integer length is what we expect it to be. If the
 222          * handler want a 16 bits integer then a 32 bits is not good enough
 223          */
 224         if (((type & PV_MASK) != PV_INTEGER) && ((type & PV_MASK) != p.pl)) {
 225                 ERROR(__FUNCTION__ "(), invalid parameter length! "
 226                       "Expected %d bytes, but value had %d bytes!\n",
 227                       type & PV_MASK, p.pl);
 228
 229                 /* Skip parameter */
 230                 return p.pl+2;
 231         }
 232
 233         switch (p.pl) {
 234         case 1:
 235                 n += irda_param_unpack(buf+2, "b", &p.pv.b);
 236                 break;
 237         case 2:
 238                 n += irda_param_unpack(buf+2, "s", &p.pv.s);
 239                 if (type & PV_BIG_ENDIAN)
 240                         be16_to_cpus(&p.pv.s);
 241                 else
 242                         le16_to_cpus(&p.pv.s);
 243                 break;
 244         case 4:
 245                 n += irda_param_unpack(buf+2, "i", &p.pv.i);
 246                 if (type & PV_BIG_ENDIAN)
 247                         be32_to_cpus(&p.pv.i);
 248                 else
 249                         le32_to_cpus(&p.pv.i);
 250                 break;
 251         default:
 252                 WARNING(__FUNCTION__ "() length %d not supported\n", p.pl);
 253
 254                 /* Skip parameter */
 255                 return p.pl+2;
 256         }
 257
 258         /* Call handler for this parameter */
 259         err = (*func)(self, &p, PV_PUT);
 260         if (err < 0)
 261                 return err;
 262
 263         return p.pl+2; /* Extracted pl+2 bytes */
 264 }
 265
 266 /*
 267  * Function irda_extract_string (self, buf, len, type, func)
 268  *
 269  *
 270  *
 271  */
 272 static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi,
 273                                PV_TYPE type, PI_HANDLER func)
 274 {
 275         char str[33];
 276         param_t p;
 277         int err;
 278
 279         IRDA_DEBUG(2, __FUNCTION__ "()\n");
 280
 281         p.pi = pi;     /* In case handler needs to know */
 282         p.pl = buf[1]; /* Extract lenght of value */
 283
 284         IRDA_DEBUG(2, __FUNCTION__ "(), pi=%#x, pl=%d\n", p.pi, p.pl);
 285
 286         /* Check if buffer is long enough for parsing */
 287         if (len < (2+p.pl)) {
 288                 WARNING(__FUNCTION__ "(), buffer to short for parsing! "
 289                         "Need %d bytes, but len is only %d\n", p.pl, len);
 290                 return -1;
 291         }
 292
 293         /* Should be safe to copy string like this since we have already
 294          * checked that the buffer is long enough */
 295         strncpy(str, buf+2, p.pl);
 296
 297         IRDA_DEBUG(2, __FUNCTION__ "(), str=0x%02x 0x%02x\n", (__u8) str[0],
 298               (__u8) str[1]);
 299
 300         /* Null terminate string */
 301         str[p.pl+1] = '\0';
 302
 303         p.pv.c = str; /* Handler will need to take a copy */
 304
 305         /* Call handler for this parameter */
 306         err = (*func)(self, &p, PV_PUT);
 307         if (err < 0)
 308                 return err;
 309
 310         return p.pl+2; /* Extracted pl+2 bytes */
 311 }
 312
 313 /*
 314  * Function irda_extract_octseq (self, buf, len, type, func)
 315  *
 316  *
 317  *
 318  */
 319 static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
 320                                PV_TYPE type, PI_HANDLER func)
 321 {
 322         param_t p;
 323
 324         p.pi = pi;     /* In case handler needs to know */
 325         p.pl = buf[1]; /* Extract lenght of value */
 326
 327         /* Check if buffer is long enough for parsing */
 328         if (len < (2+p.pl)) {
 329                 WARNING(__FUNCTION__ "(), buffer to short for parsing! "
 330                         "Need %d bytes, but len is only %d\n", p.pl, len);
 331                 return -1;
 332         }
 333
 334         IRDA_DEBUG(0, __FUNCTION__ "(), not impl\n");
 335
 336         return p.pl+2; /* Extracted pl+2 bytes */
 337 }
 338
 339 /*
 340  * Function irda_param_pack (skb, fmt, ...)
 341  *
 342  *    Format:
 343  *        'i' = 32 bits integer
 344  *        's' = string
 345  *
 346  */
 347 int irda_param_pack(__u8 *buf, char *fmt, ...)
 348 {
 349         va_list args;
 350         char *p;
 351         int n = 0;
 352         pv_t arg;
 353
 354         va_start(args, fmt);
 355
 356         for (p = fmt; *p != '\0'; p++) {
 357                 switch (*p) {
 358                 case 'b':  /* 8 bits unsigned byte */
 359                         buf[n++] = va_arg(args, __u8);
 360                         break;
 361                 case 's':  /* 16 bits unsigned short */
 362                         arg.s = va_arg(args, __u16);
 363                         put_unaligned(arg.s, (__u16 *)(buf+n)); n+=2;
 364                         break;
 365                 case 'i':  /* 32 bits unsigned integer */
 366                         arg.i = va_arg(args, __u32);
 367                         put_unaligned(arg.i, (__u32 *)(buf+n)); n+=4;
 368                         break;
 369                 case 'c': /* \0 terminated string */
 370                         arg.c = va_arg(args, char *);
 371                         strcpy(buf+n, arg.c);
 372                         n += strlen(arg.c) + 1;
 373                         break;
 374                 default:
 375                         va_end(args);
 376                         return -1;
 377                 }
 378
 379         }
 380         va_end(args);
 381
 382         return 0;
 383 }
 384
 385 /*
 386  * Function irda_param_unpack (skb, fmt, ...)
 387  *
 388  *
 389  *
 390  */
 391 int irda_param_unpack(__u8 *buf, char *fmt, ...)
 392 {
 393         va_list args;
 394         char *p;
 395         int n = 0;
 396         pv_t arg;
 397
 398         va_start(args, fmt);
 399
 400         for (p = fmt; *p != '\0'; p++) {
 401                 switch (*p) {
 402                 case 'b':  /* 8 bits byte */
 403                         arg.bp = va_arg(args, __u8 *);
 404                         *arg.bp = buf[n++];
 405                         break;
 406                 case 's':  /* 16 bits short */
 407                         arg.sp = va_arg(args, __u16 *);
 408                         *arg.sp = get_unaligned((__u16 *)(buf+n)); n+=2;
 409                         break;
 410                 case 'i':  /* 32 bits unsigned integer */
 411                         arg.ip = va_arg(args, __u32 *);
 412                         *arg.ip = get_unaligned((__u32 *)(buf+n)); n+=4;
 413                         break;
 414 #if 0
 415                 case 'c':   /* \0 terminated string */
 416                         arg.c = va_arg(args, char *);
 417                         strcpy(arg.c, buf+n);
 418                         n += strlen(arg.c) + 1;
 419                         break;
 420 #endif
 421                 default:
 422                         va_end(args);
 423                         return -1;
 424                 }
 425
 426         }
 427         va_end(args);
 428
 429         return 0;
 430 }
 431
 432 /*
 433  * Function irda_param_insert (self, pi, buf, len, info)
 434  *
 435  *    Insert the specified parameter (pi) into buffer. Returns number of
 436  *    bytes inserted
 437  */
 438 int irda_param_insert(void *self, __u8 pi, __u8 *buf, int len,
 439                       pi_param_info_t *info)
 440 {
 441         pi_minor_info_t *pi_minor_info;
 442         __u8 pi_minor;
 443         __u8 pi_major;
 444         int type;
 445         int ret = -1;
 446         int n = 0;
 447
 448         ASSERT(buf != NULL, return ret;);
 449         ASSERT(info != 0, return ret;);
 450
 451         pi_minor = pi & info->pi_mask;
 452         pi_major = pi >> info->pi_major_offset;
 453
 454         /* Check if the identifier value (pi) is valid */
 455         if ((pi_major > info->len-1) ||
 456             (pi_minor > info->tables[pi_major].len-1))
 457         {
 458                 IRDA_DEBUG(0, __FUNCTION__
 459                       "(), no handler for parameter=0x%02x\n", pi);
 460
 461                 /* Skip this parameter */
 462                 return -1;
 463         }
 464
 465         /* Lookup the info on how to parse this parameter */
 466         pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
 467
 468         /* Find expected data type for this parameter identifier (pi)*/
 469         type = pi_minor_info->type;
 470
 471         IRDA_DEBUG(3, __FUNCTION__ "(), pi=[%d,%d], type=%d\n",
 472               pi_major, pi_minor, type);
 473
 474         /*  Check if handler has been implemented */
 475         if (!pi_minor_info->func) {
 476                 MESSAGE(__FUNCTION__"(), no handler for pi=%#x\n", pi);
 477                 /* Skip this parameter */
 478                 return -1;
 479         }
 480
 481         /* Insert parameter value */
 482         ret = (*pv_insert_table[type & PV_MASK])(self, buf+n, len, pi, type,
 483                                                  pi_minor_info->func);
 484         return ret;
 485 }
 486
 487 /*
 488  * Function irda_param_extract_all (self, buf, len, info)
 489  *
 490  *    Parse all parameters. If len is correct, then everything should be
 491  *    safe. Returns the number of bytes that was parsed
 492  *
 493  */
 494 int irda_param_extract(void *self, __u8 *buf, int len, pi_param_info_t *info)
 495 {
 496         pi_minor_info_t *pi_minor_info;
 497         __u8 pi_minor;
 498         __u8 pi_major;
 499         int type;
 500         int ret = -1;
 501         int n = 0;
 502
 503         ASSERT(buf != NULL, return ret;);
 504         ASSERT(info != 0, return ret;);
 505
 506         pi_minor = buf[n] & info->pi_mask;
 507         pi_major = buf[n] >> info->pi_major_offset;
 508
 509         /* Check if the identifier value (pi) is valid */
 510         if ((pi_major > info->len-1) ||
 511             (pi_minor > info->tables[pi_major].len-1))
 512         {
 513                 IRDA_DEBUG(0, __FUNCTION__ "(), no handler for parameter=0x%02x\n",
 514                       buf[0]);
 515
 516                 /* Skip this parameter */
 517                 n += (2 + buf[n+1]);
 518                 len -= (2 + buf[n+1]);
 519
 520                 return 0;  /* Continue */
 521         }
 522
 523         /* Lookup the info on how to parse this parameter */
 524         pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
 525
 526         /* Find expected data type for this parameter identifier (pi)*/
 527         type = pi_minor_info->type;
 528
 529         IRDA_DEBUG(3, __FUNCTION__ "(), pi=[%d,%d], type=%d\n",
 530               pi_major, pi_minor, type);
 531
 532         /*  Check if handler has been implemented */
 533         if (!pi_minor_info->func) {
 534                 MESSAGE(__FUNCTION__"(), no handler for pi=%#x\n", buf[n]);
 535                 /* Skip this parameter */
 536                 n += (2 + buf[n+1]);
 537                 len -= (2 + buf[n+1]);
 538
 539                 return 0; /* Continue */
 540         }
 541
 542         /* Parse parameter value */
 543         ret = (*pv_extract_table[type & PV_MASK])(self, buf+n, len, buf[n],
 544                                                   type, pi_minor_info->func);
 545         return ret;
 546 }
 547
 548 /*
 549  * Function irda_param_extract_all (self, buf, len, info)
 550  *
 551  *    Parse all parameters. If len is correct, then everything should be
 552  *    safe. Returns the number of bytes that was parsed
 553  *
 554  */
 555 int irda_param_extract_all(void *self, __u8 *buf, int len,
 556                            pi_param_info_t *info)
 557 {
 558         int ret = -1;
 559         int n = 0;
 560
 561         ASSERT(buf != NULL, return ret;);
 562         ASSERT(info != 0, return ret;);
 563
 564         /*
 565          * Parse all parameters. Each parameter must be at least two bytes
 566          * long or else there is no point in trying to parse it
 567          */
 568         while (len > 2) {
 569                 ret = irda_param_extract(self, buf+n, len, info);
 570                 if (ret < 0)
 571                         return ret;
 572
 573                 n += ret;
 574                 len -= ret;
 575         }
 576         return n;
 577 }
 578