LXR linux/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
  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