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 */
irda_insert_no_value(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)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 */
irda_extract_no_value(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)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 */
irda_insert_integer(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)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 */
irda_extract_integer(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)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 */
irda_extract_string(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)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 */
irda_extract_octseq(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)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 */
irda_param_pack(__u8 * buf,char * fmt,...)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 */
irda_param_unpack(__u8 * buf,char * fmt,...)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 */
irda_param_insert(void * self,__u8 pi,__u8 * buf,int len,pi_param_info_t * info)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 */
irda_param_extract(void * self,__u8 * buf,int len,pi_param_info_t * info)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 */
irda_param_extract_all(void * self,__u8 * buf,int len,pi_param_info_t * info)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);
592