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 <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 /* Parameter value call table. Must match PV_TYPE */
53 static PV_HANDLER pv_extract_table[] = {
54 irda_extract_integer, /* Handler for any length integers */
55 irda_extract_integer, /* Handler for 8 bits integers */
56 irda_extract_integer, /* Handler for 16 bits integers */
57 irda_extract_string, /* Handler for strings */
58 irda_extract_integer, /* Handler for 32 bits integers */
59 irda_extract_octseq, /* Handler for octet sequences */
60 irda_extract_no_value /* Handler for no value parameters */
61 };
62
63 static PV_HANDLER pv_insert_table[] = {
64 irda_insert_integer, /* Handler for any length integers */
65 irda_insert_integer, /* Handler for 8 bits integers */
66 irda_insert_integer, /* Handler for 16 bits integers */
67 NULL, /* Handler for strings */
68 irda_insert_integer, /* Handler for 32 bits integers */
69 NULL, /* Handler for octet sequences */
70 irda_insert_no_value /* Handler for no value parameters */
71 };
72
73 /*
74 * Function irda_insert_no_value (self, buf, len, pi, type, func)
75 *
76 *
77 *
78 */
irda_insert_no_value(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)79 static int irda_insert_no_value(void *self, __u8 *buf, int len, __u8 pi,
80 PV_TYPE type, PI_HANDLER func)
81 {
82 irda_param_t p;
83 int ret;
84
85 p.pi = pi;
86 p.pl = 0;
87
88 /* Call handler for this parameter */
89 ret = (*func)(self, &p, PV_GET);
90
91 /* Extract values anyway, since handler may need them */
92 irda_param_pack(buf, "bb", p.pi, p.pl);
93
94 if (ret < 0)
95 return ret;
96
97 return 2; /* Inserted pl+2 bytes */
98 }
99
100 /*
101 * Function irda_extract_no_value (self, buf, len, type, func)
102 *
103 * Extracts a parameter without a pv field (pl=0)
104 *
105 */
irda_extract_no_value(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)106 static int irda_extract_no_value(void *self, __u8 *buf, int len, __u8 pi,
107 PV_TYPE type, PI_HANDLER func)
108 {
109 irda_param_t p;
110 int ret;
111
112 /* Extract values anyway, since handler may need them */
113 irda_param_unpack(buf, "bb", &p.pi, &p.pl);
114
115 /* Call handler for this parameter */
116 ret = (*func)(self, &p, PV_PUT);
117
118 if (ret < 0)
119 return ret;
120
121 return 2; /* Extracted pl+2 bytes */
122 }
123
124 /*
125 * Function irda_insert_integer (self, buf, len, pi, type, func)
126 *
127 *
128 *
129 */
irda_insert_integer(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)130 static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi,
131 PV_TYPE type, PI_HANDLER func)
132 {
133 irda_param_t p;
134 int n = 0;
135 int err;
136
137 p.pi = pi; /* In case handler needs to know */
138 p.pl = type & PV_MASK; /* The integer type codes the lenght as well */
139 p.pv.i = 0; /* Clear value */
140
141 /* Call handler for this parameter */
142 err = (*func)(self, &p, PV_GET);
143 if (err < 0)
144 return err;
145
146 /*
147 * If parameter lenght is still 0, then (1) this is an any length
148 * integer, and (2) the handler function does not care which length
149 * we choose to use, so we pick the one the gives the fewest bytes.
150 */
151 if (p.pl == 0) {
152 if (p.pv.i < 0xff) {
153 IRDA_DEBUG(2, "%s(), using 1 byte\n", __FUNCTION__);
154 p.pl = 1;
155 } else if (p.pv.i < 0xffff) {
156 IRDA_DEBUG(2, "%s(), using 2 bytes\n", __FUNCTION__);
157 p.pl = 2;
158 } else {
159 IRDA_DEBUG(2, "%s(), using 4 bytes\n", __FUNCTION__);
160 p.pl = 4; /* Default length */
161 }
162 }
163 /* Check if buffer is long enough for insertion */
164 if (len < (2+p.pl)) {
165 WARNING("%s(), buffer to short for insertion!\n", __FUNCTION__);
166 return -1;
167 }
168 IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d, pi=%d\n", __FUNCTION__, 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 WARNING("%s() length %d not supported\n", __FUNCTION__, p.pl);
190 /* Skip parameter */
191 return -1;
192 }
193
194 return p.pl+2; /* Inserted pl+2 bytes */
195 }
196
197 /*
198 * Function irda_extract integer (self, buf, len, pi, type, func)
199 *
200 * Extract a possibly variable length integer from buffer, and call
201 * handler for processing of the parameter
202 */
irda_extract_integer(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)203 static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi,
204 PV_TYPE type, PI_HANDLER func)
205 {
206 irda_param_t p;
207 int n = 0;
208 int extract_len; /* Real lenght we extract */
209 int err;
210
211 p.pi = pi; /* In case handler needs to know */
212 p.pl = buf[1]; /* Extract lenght of value */
213 p.pv.i = 0; /* Clear value */
214 extract_len = p.pl; /* Default : extract all */
215
216 /* Check if buffer is long enough for parsing */
217 if (len < (2+p.pl)) {
218 WARNING("%s(), buffer to short for parsing! "
219 "Need %d bytes, but len is only %d\n", __FUNCTION__, p.pl, len);
220 return -1;
221 }
222
223 /*
224 * Check that the integer length is what we expect it to be. If the
225 * handler want a 16 bits integer then a 32 bits is not good enough
226 * PV_INTEGER means that the handler is flexible.
227 */
228 if (((type & PV_MASK) != PV_INTEGER) && ((type & PV_MASK) != p.pl)) {
229 ERROR("%s(), invalid parameter length! "
230 "Expected %d bytes, but value had %d bytes!\n",
231 __FUNCTION__, type & PV_MASK, p.pl);
232
233 /* Most parameters are bit/byte fields or little endian,
234 * so it's ok to only extract a subset of it (the subset
235 * that the handler expect). This is necessary, as some
236 * broken implementations seems to add extra undefined bits.
237 * If the parameter is shorter than we expect or is big
238 * endian, we can't play those tricks. Jean II */
239 if((p.pl < (type & PV_MASK)) || (type & PV_BIG_ENDIAN)) {
240 /* Skip parameter */
241 return p.pl+2;
242 } else {
243 /* Extract subset of it, fallthrough */
244 extract_len = type & PV_MASK;
245 }
246 }
247
248
249 switch (extract_len) {
250 case 1:
251 n += irda_param_unpack(buf+2, "b", &p.pv.i);
252 break;
253 case 2:
254 n += irda_param_unpack(buf+2, "s", &p.pv.i);
255 if (type & PV_BIG_ENDIAN)
256 p.pv.i = be16_to_cpu((__u16) p.pv.i);
257 else
258 p.pv.i = le16_to_cpu((__u16) p.pv.i);
259 break;
260 case 4:
261 n += irda_param_unpack(buf+2, "i", &p.pv.i);
262 if (type & PV_BIG_ENDIAN)
263 be32_to_cpus(&p.pv.i);
264 else
265 le32_to_cpus(&p.pv.i);
266 break;
267 default:
268 WARNING("%s() length %d not supported\n", __FUNCTION__, p.pl);
269
270 /* Skip parameter */
271 return p.pl+2;
272 }
273
274 IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d, pi=%d\n", __FUNCTION__, 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 *
286 *
287 *
288 */
irda_extract_string(void * self,__u8 * buf,int len,__u8 pi,PV_TYPE type,PI_HANDLER func)289 static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi,
290 PV_TYPE type, PI_HANDLER func)
291 {
292 char str[33];
293 irda_param_t p;
294 int err;
295
296 IRDA_DEBUG(2, "%s()\n", __FUNCTION__);
297
298 p.pi = pi; /* In case handler needs to know */
299 p.pl = buf[1]; /* Extract lenght of value */
300
301 IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d\n", __FUNCTION__, p.pi, p.pl);
302
303 /* Check if buffer is long enough for parsing */
304 if (len < (2+p.pl)) {
305 WARNING("%s(), buffer to short for parsing! "
306 "Need %d bytes, but len is only %d\n", __FUNCTION__, p.pl, len);
307 return -1;
308 }
309
310 /* Should be safe to copy string like this since we have already
311 * checked that the buffer is long enough */
312 strncpy(str, buf+2, p.pl);
313
314 IRDA_DEBUG(2, "%s(), str=0x%02x 0x%02x\n", __FUNCTION__, (__u8) str[0],
315 (__u8) str[1]);
316
317 /* Null terminate string */
318 str[p.pl+1] = '\0';
319
320 p.pv.c = str; /* Handler will need to take a copy */
321
322 /* Call handler for this parameter */
323 err = (*func)(self, &p, PV_PUT);
324 if (err < 0)
325 return err;
326
327 return p.pl+2; /* Extracted pl+2 bytes */
328 }
329
330 /*
331 * Function irda_extract_octseq (self, buf, len, type, func)
332 *
333 *
334 *
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 lenght of value */
343
344 /* Check if buffer is long enough for parsing */
345 if (len < (2+p.pl)) {
346 WARNING("%s(), buffer to short for parsing! "
347 "Need %d bytes, but len is only %d\n", __FUNCTION__, p.pl, len);
348 return -1;
349 }
350
351 IRDA_DEBUG(0, "%s(), not impl\n", __FUNCTION__);
352
353 return p.pl+2; /* Extracted pl+2 bytes */
354 }
355
356 /*
357 * Function irda_param_pack (skb, fmt, ...)
358 *
359 * Format:
360 * 'i' = 32 bits integer
361 * 's' = string
362 *
363 */
irda_param_pack(__u8 * buf,char * fmt,...)364 int irda_param_pack(__u8 *buf, char *fmt, ...)
365 {
366 irda_pv_t arg;
367 va_list args;
368 char *p;
369 int n = 0;
370
371 va_start(args, fmt);
372
373 for (p = fmt; *p != '\0'; p++) {
374 switch (*p) {
375 case 'b': /* 8 bits unsigned byte */
376 buf[n++] = (__u8)va_arg(args, int);
377 break;
378 case 's': /* 16 bits unsigned short */
379 arg.i = (__u16)va_arg(args, int);
380 put_unaligned((__u16)arg.i, (__u16 *)(buf+n)); n+=2;
381 break;
382 case 'i': /* 32 bits unsigned integer */
383 arg.i = va_arg(args, __u32);
384 put_unaligned(arg.i, (__u32 *)(buf+n)); n+=4;
385 break;
386 #if 0
387 case 'c': /* \0 terminated string */
388 arg.c = va_arg(args, char *);
389 strcpy(buf+n, arg.c);
390 n += strlen(arg.c) + 1;
391 break;
392 #endif
393 default:
394 va_end(args);
395 return -1;
396 }
397
398 }
399 va_end(args);
400
401 return 0;
402 }
403
404 /*
405 * Function irda_param_unpack (skb, fmt, ...)
406 *
407 *
408 *
409 */
irda_param_unpack(__u8 * buf,char * fmt,...)410 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 ASSERT(buf != NULL, return ret;);
468 ASSERT(info != 0, 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", __FUNCTION__, pi);
478
479 /* Skip this parameter */
480 return -1;
481 }
482
483 /* Lookup the info on how to parse this parameter */
484 pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
485
486 /* Find expected data type for this parameter identifier (pi)*/
487 type = pi_minor_info->type;
488
489 /* Check if handler has been implemented */
490 if (!pi_minor_info->func) {
491 MESSAGE("%s(), no handler for pi=%#x\n", __FUNCTION__, pi);
492 /* Skip this parameter */
493 return -1;
494 }
495
496 /* Insert parameter value */
497 ret = (*pv_insert_table[type & PV_MASK])(self, buf+n, len, pi, type,
498 pi_minor_info->func);
499 return ret;
500 }
501
502 /*
503 * Function irda_param_extract_all (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 int irda_param_extract(void *self, __u8 *buf, int len, pi_param_info_t *info)
510 {
511 pi_minor_info_t *pi_minor_info;
512 __u8 pi_minor;
513 __u8 pi_major;
514 int type;
515 int ret = -1;
516 int n = 0;
517
518 ASSERT(buf != NULL, return ret;);
519 ASSERT(info != 0, return ret;);
520
521 pi_minor = buf[n] & info->pi_mask;
522 pi_major = buf[n] >> info->pi_major_offset;
523
524 /* Check if the identifier value (pi) is valid */
525 if ((pi_major > info->len-1) ||
526 (pi_minor > info->tables[pi_major].len-1))
527 {
528 IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n", __FUNCTION__,
529 buf[0]);
530
531 /* Skip this parameter */
532 return 2 + buf[n + 1]; /* Continue */
533 }
534
535 /* Lookup the info on how to parse this parameter */
536 pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
537
538 /* Find expected data type for this parameter identifier (pi)*/
539 type = pi_minor_info->type;
540
541 IRDA_DEBUG(3, "%s(), pi=[%d,%d], type=%d\n", __FUNCTION__,
542 pi_major, pi_minor, type);
543
544 /* Check if handler has been implemented */
545 if (!pi_minor_info->func) {
546 MESSAGE("%s(), no handler for pi=%#x\n", __FUNCTION__, buf[n]);
547 /* Skip this parameter */
548 return 2 + buf[n + 1]; /* Continue */
549 }
550
551 /* Parse parameter value */
552 ret = (*pv_extract_table[type & PV_MASK])(self, buf+n, len, buf[n],
553 type, pi_minor_info->func);
554 return ret;
555 }
556
557 /*
558 * Function irda_param_extract_all (self, buf, len, info)
559 *
560 * Parse all parameters. If len is correct, then everything should be
561 * safe. Returns the number of bytes that was parsed
562 *
563 */
irda_param_extract_all(void * self,__u8 * buf,int len,pi_param_info_t * info)564 int irda_param_extract_all(void *self, __u8 *buf, int len,
565 pi_param_info_t *info)
566 {
567 int ret = -1;
568 int n = 0;
569
570 ASSERT(buf != NULL, return ret;);
571 ASSERT(info != 0, return ret;);
572
573 /*
574 * Parse all parameters. Each parameter must be at least two bytes
575 * long or else there is no point in trying to parse it
576 */
577 while (len > 2) {
578 ret = irda_param_extract(self, buf+n, len, info);
579 if (ret < 0)
580 return ret;
581
582 n += ret;
583 len -= ret;
584 }
585 return n;
586 }
587
588