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