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
302 IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d\n", __func__,
303 p.pi, p.pl);
304
305 /* Check if buffer is long enough for parsing */
306 if (len < (2+p.pl)) {
307 IRDA_WARNING("%s: buffer too short for parsing! "
308 "Need %d bytes, but len is only %d\n",
309 __func__, p.pl, len);
310 return -1;
311 }
312
313 /* Should be safe to copy string like this since we have already
314 * checked that the buffer is long enough */
315 strncpy(str, buf+2, p.pl);
316
317 IRDA_DEBUG(2, "%s(), str=0x%02x 0x%02x\n", __func__,
318 (__u8) str[0], (__u8) str[1]);
319
320 /* Null terminate string */
321 str[p.pl+1] = '\0';
322
323 p.pv.c = str; /* Handler will need to take a copy */
324
325 /* Call handler for this parameter */
326 err = (*func)(self, &p, PV_PUT);
327 if (err < 0)
328 return err;
329
330 return p.pl+2; /* Extracted pl+2 bytes */
331 }
332
333 /*
334 * Function irda_extract_octseq (self, buf, len, type, func)
335 */
irda_extract_octseq(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)336 static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
337 PV_TYPE type, PI_HANDLER func)
338 {
339 irda_param_t p;
340
341 p.pi = pi; /* In case handler needs to know */
342 p.pl = buf[1]; /* Extract length of value */
343
344 /* Check if buffer is long enough for parsing */
345 if (len < (2+p.pl)) {
346 IRDA_WARNING("%s: buffer too short for parsing! "
347 "Need %d bytes, but len is only %d\n",
348 __func__, p.pl, len);
349 return -1;
350 }
351
352 IRDA_DEBUG(0, "%s(), not impl\n", __func__);
353
354 return p.pl+2; /* Extracted pl+2 bytes */
355 }
356
357 /*
358 * Function irda_param_pack (skb, fmt, ...)
359 *
360 * Format:
361 * 'i' = 32 bits integer
362 * 's' = string
363 *
364 */
irda_param_pack(__u8 * buf,char * fmt,...)365 int irda_param_pack(__u8 *buf, char *fmt, ...)
366 {
367 irda_pv_t arg;
368 va_list args;
369 char *p;
370 int n = 0;
371
372 va_start(args, fmt);
373
374 for (p = fmt; *p != '\0'; p++) {
375 switch (*p) {
376 case 'b': /* 8 bits unsigned byte */
377 buf[n++] = (__u8)va_arg(args, int);
378 break;
379 case 's': /* 16 bits unsigned short */
380 arg.i = (__u16)va_arg(args, int);
381 put_unaligned((__u16)arg.i, (__u16 *)(buf+n)); n+=2;
382 break;
383 case 'i': /* 32 bits unsigned integer */
384 arg.i = va_arg(args, __u32);
385 put_unaligned(arg.i, (__u32 *)(buf+n)); n+=4;
386 break;
387 #if 0
388 case 'c': /* \0 terminated string */
389 arg.c = va_arg(args, char *);
390 strcpy(buf+n, arg.c);
391 n += strlen(arg.c) + 1;
392 break;
393 #endif
394 default:
395 va_end(args);
396 return -1;
397 }
398 }
399 va_end(args);
400
401 return 0;
402 }
403 EXPORT_SYMBOL(irda_param_pack);
404
405 /*
406 * Function irda_param_unpack (skb, fmt, ...)
407 */
irda_param_unpack(__u8 * buf,char * fmt,...)408 static int irda_param_unpack(__u8 *buf, char *fmt, ...)
409 {
410 irda_pv_t arg;
411 va_list args;
412 char *p;
413 int n = 0;
414
415 va_start(args, fmt);
416
417 for (p = fmt; *p != '\0'; p++) {
418 switch (*p) {
419 case 'b': /* 8 bits byte */
420 arg.ip = va_arg(args, __u32 *);
421 *arg.ip = buf[n++];
422 break;
423 case 's': /* 16 bits short */
424 arg.ip = va_arg(args, __u32 *);
425 *arg.ip = get_unaligned((__u16 *)(buf+n)); n+=2;
426 break;
427 case 'i': /* 32 bits unsigned integer */
428 arg.ip = va_arg(args, __u32 *);
429 *arg.ip = get_unaligned((__u32 *)(buf+n)); n+=4;
430 break;
431 #if 0
432 case 'c': /* \0 terminated string */
433 arg.c = va_arg(args, char *);
434 strcpy(arg.c, buf+n);
435 n += strlen(arg.c) + 1;
436 break;
437 #endif
438 default:
439 va_end(args);
440 return -1;
441 }
442
443 }
444 va_end(args);
445
446 return 0;
447 }
448
449 /*
450 * Function irda_param_insert (self, pi, buf, len, info)
451 *
452 * Insert the specified parameter (pi) into buffer. Returns number of
453 * bytes inserted
454 */
irda_param_insert(void * self,__u8 pi,__u8 * buf,int len,pi_param_info_t * info)455 int irda_param_insert(void *self, __u8 pi, __u8 *buf, int len,
456 pi_param_info_t *info)
457 {
458 pi_minor_info_t *pi_minor_info;
459 __u8 pi_minor;
460 __u8 pi_major;
461 int type;
462 int ret = -1;
463 int n = 0;
464
465 IRDA_ASSERT(buf != NULL, return ret;);
466 IRDA_ASSERT(info != NULL, return ret;);
467
468 pi_minor = pi & info->pi_mask;
469 pi_major = pi >> info->pi_major_offset;
470
471 /* Check if the identifier value (pi) is valid */
472 if ((pi_major > info->len-1) ||
473 (pi_minor > info->tables[pi_major].len-1))
474 {
475 IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n",
476 __func__, pi);
477
478 /* Skip this parameter */
479 return -1;
480 }
481
482 /* Lookup the info on how to parse this parameter */
483 pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
484
485 /* Find expected data type for this parameter identifier (pi)*/
486 type = pi_minor_info->type;
487
488 /* Check if handler has been implemented */
489 if (!pi_minor_info->func) {
490 IRDA_MESSAGE("%s: no handler for pi=%#x\n", __func__, pi);
491 /* Skip this parameter */
492 return -1;
493 }
494
495 /* Insert parameter value */
496 ret = (*pv_insert_table[type & PV_MASK])(self, buf+n, len, pi, type,
497 pi_minor_info->func);
498 return ret;
499 }
500 EXPORT_SYMBOL(irda_param_insert);
501
502 /*
503 * Function irda_param_extract (self, buf, len, info)
504 *
505 * Parse all parameters. If len is correct, then everything should be
506 * safe. Returns the number of bytes that was parsed
507 *
508 */
irda_param_extract(void * self,__u8 * buf,int len,pi_param_info_t * info)509 static int irda_param_extract(void *self, __u8 *buf, int len,
510 pi_param_info_t *info)
511 {
512 pi_minor_info_t *pi_minor_info;
513 __u8 pi_minor;
514 __u8 pi_major;
515 int type;
516 int ret = -1;
517 int n = 0;
518
519 IRDA_ASSERT(buf != NULL, return ret;);
520 IRDA_ASSERT(info != NULL, return ret;);
521
522 pi_minor = buf[n] & info->pi_mask;
523 pi_major = buf[n] >> info->pi_major_offset;
524
525 /* Check if the identifier value (pi) is valid */
526 if ((pi_major > info->len-1) ||
527 (pi_minor > info->tables[pi_major].len-1))
528 {
529 IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n",
530 __func__, buf[0]);
531
532 /* Skip this parameter */
533 return 2 + buf[n + 1]; /* Continue */
534 }
535
536 /* Lookup the info on how to parse this parameter */
537 pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
538
539 /* Find expected data type for this parameter identifier (pi)*/
540 type = pi_minor_info->type;
541
542 IRDA_DEBUG(3, "%s(), pi=[%d,%d], type=%d\n", __func__,
543 pi_major, pi_minor, type);
544
545 /* Check if handler has been implemented */
546 if (!pi_minor_info->func) {
547 IRDA_MESSAGE("%s: no handler for pi=%#x\n",
548 __func__, buf[n]);
549 /* Skip this parameter */
550 return 2 + buf[n + 1]; /* Continue */
551 }
552
553 /* Parse parameter value */
554 ret = (*pv_extract_table[type & PV_MASK])(self, buf+n, len, buf[n],
555 type, pi_minor_info->func);
556 return ret;
557 }
558
559 /*
560 * Function irda_param_extract_all (self, buf, len, info)
561 *
562 * Parse all parameters. If len is correct, then everything should be
563 * safe. Returns the number of bytes that was parsed
564 *
565 */
irda_param_extract_all(void * self,__u8 * buf,int len,pi_param_info_t * info)566 int irda_param_extract_all(void *self, __u8 *buf, int len,
567 pi_param_info_t *info)
568 {
569 int ret = -1;
570 int n = 0;
571
572 IRDA_ASSERT(buf != NULL, return ret;);
573 IRDA_ASSERT(info != NULL, return ret;);
574
575 /*
576 * Parse all parameters. Each parameter must be at least two bytes
577 * long or else there is no point in trying to parse it
578 */
579 while (len > 2) {
580 ret = irda_param_extract(self, buf+n, len, info);
581 if (ret < 0)
582 return ret;
583
584 n += ret;
585 len -= ret;
586 }
587 return n;
588 }
589 EXPORT_SYMBOL(irda_param_extract_all);
590