Loading...
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);
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);