cnic: Add kcq2 support on 57712
[linux-2.6.git] / 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 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  */
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 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  */
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", __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  */
338 static 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  */
367 int 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 }
405 EXPORT_SYMBOL(irda_param_pack);
406
407 /*
408  * Function irda_param_unpack (skb, fmt, ...)
409  */
410 static 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  */
457 int 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 }
502 EXPORT_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  */
511 static 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  */
568 int 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 }
591 EXPORT_SYMBOL(irda_param_extract_all);