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
38 static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi,
39 PV_TYPE type, PI_HANDLER func);
40 static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi,
41 PV_TYPE type, PI_HANDLER func);
42 static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
43 PV_TYPE type, PI_HANDLER func);
44 static int irda_extract_no_value(void *self, __u8 *buf, int len, __u8 pi,
45 PV_TYPE type, PI_HANDLER func);
46
47 static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi,
48 PV_TYPE type, PI_HANDLER func);
49 static int irda_insert_no_value(void *self, __u8 *buf, int len, __u8 pi,
50 PV_TYPE type, PI_HANDLER func);
51
52 static int irda_param_unpack(__u8 *buf, char *fmt, ...);
53
54 /* Parameter value call table. Must match PV_TYPE */
55 static 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
65 static 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 */
irda_insert_no_value(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)78 static 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 */
irda_extract_no_value(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)105 static 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 */
irda_insert_integer(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)126 static 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 */
irda_extract_integer(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)202 static 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 */
irda_extract_string(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)286 static 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 */
irda_extract_octseq(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)331 static 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 */
irda_param_pack(__u8 * buf,char * fmt,...)359 int 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 }
397 EXPORT_SYMBOL(irda_param_pack);
398
399 /*
400 * Function irda_param_unpack (skb, fmt, ...)
401 */
irda_param_unpack(__u8 * buf,char * fmt,...)402 static 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 */
irda_param_insert(void * self,__u8 pi,__u8 * buf,int len,pi_param_info_t * info)449 int 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 }
495 EXPORT_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 */
irda_param_extract(void * self,__u8 * buf,int len,pi_param_info_t * info)504 static 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 */
irda_param_extract_all(void * self,__u8 * buf,int len,pi_param_info_t * info)561 int 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 }
584 EXPORT_SYMBOL(irda_param_extract_all);
585