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