1 /*********************************************************************
3 * Filename: parameters.c
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>
12 * Copyright (c) 1999-2000 Dag Brattli, All Rights Reserved.
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.
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.
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,
29 ********************************************************************/
31 #include <linux/types.h>
32 #include <linux/module.h>
34 #include <asm/unaligned.h>
35 #include <asm/byteorder.h>
37 #include <net/irda/irda.h>
38 #include <net/irda/parameters.h>
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);
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);
54 static int irda_param_unpack(__u8 *buf, char *fmt, ...);
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 */
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 */
78 * Function irda_insert_no_value (self, buf, len, pi, type, func)
80 static int irda_insert_no_value(void *self, __u8 *buf, int len, __u8 pi,
81 PV_TYPE type, PI_HANDLER func)
89 /* Call handler for this parameter */
90 ret = (*func)(self, &p, PV_GET);
92 /* Extract values anyway, since handler may need them */
93 irda_param_pack(buf, "bb", p.pi, p.pl);
98 return 2; /* Inserted pl+2 bytes */
102 * Function irda_extract_no_value (self, buf, len, type, func)
104 * Extracts a parameter without a pv field (pl=0)
107 static int irda_extract_no_value(void *self, __u8 *buf, int len, __u8 pi,
108 PV_TYPE type, PI_HANDLER func)
113 /* Extract values anyway, since handler may need them */
114 irda_param_unpack(buf, "bb", &p.pi, &p.pl);
116 /* Call handler for this parameter */
117 ret = (*func)(self, &p, PV_PUT);
122 return 2; /* Extracted pl+2 bytes */
126 * Function irda_insert_integer (self, buf, len, pi, type, func)
128 static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi,
129 PV_TYPE type, PI_HANDLER func)
135 p.pi = pi; /* In case handler needs to know */
136 p.pl = type & PV_MASK; /* The integer type codes the lenght as well */
137 p.pv.i = 0; /* Clear value */
139 /* Call handler for this parameter */
140 err = (*func)(self, &p, PV_GET);
145 * If parameter lenght 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.
151 IRDA_DEBUG(2, "%s(), using 1 byte\n", __FUNCTION__);
153 } else if (p.pv.i < 0xffff) {
154 IRDA_DEBUG(2, "%s(), using 2 bytes\n", __FUNCTION__);
157 IRDA_DEBUG(2, "%s(), using 4 bytes\n", __FUNCTION__);
158 p.pl = 4; /* Default length */
161 /* Check if buffer is long enough for insertion */
162 if (len < (2+p.pl)) {
163 WARNING("%s: buffer to short for insertion!\n", __FUNCTION__);
166 IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d, pi=%d\n", __FUNCTION__,
170 n += irda_param_pack(buf, "bbb", p.pi, p.pl, (__u8) p.pv.i);
173 if (type & PV_BIG_ENDIAN)
174 p.pv.i = cpu_to_be16((__u16) p.pv.i);
176 p.pv.i = cpu_to_le16((__u16) p.pv.i);
177 n += irda_param_pack(buf, "bbs", p.pi, p.pl, (__u16) p.pv.i);
180 if (type & PV_BIG_ENDIAN)
181 cpu_to_be32s(&p.pv.i);
183 cpu_to_le32s(&p.pv.i);
184 n += irda_param_pack(buf, "bbi", p.pi, p.pl, p.pv.i);
188 WARNING("%s: length %d not supported\n", __FUNCTION__, p.pl);
193 return p.pl+2; /* Inserted pl+2 bytes */
197 * Function irda_extract integer (self, buf, len, pi, type, func)
199 * Extract a possibly variable length integer from buffer, and call
200 * handler for processing of the parameter
202 static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi,
203 PV_TYPE type, PI_HANDLER func)
207 int extract_len; /* Real lenght we extract */
210 p.pi = pi; /* In case handler needs to know */
211 p.pl = buf[1]; /* Extract lenght of value */
212 p.pv.i = 0; /* Clear value */
213 extract_len = p.pl; /* Default : extract all */
215 /* Check if buffer is long enough for parsing */
216 if (len < (2+p.pl)) {
217 WARNING("%s: buffer to short for parsing! "
218 "Need %d bytes, but len is only %d\n",
219 __FUNCTION__, p.pl, len);
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.
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);
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)) {
243 /* Extract subset of it, fallthrough */
244 extract_len = type & PV_MASK;
249 switch (extract_len) {
251 n += irda_param_unpack(buf+2, "b", &p.pv.i);
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);
258 p.pv.i = le16_to_cpu((__u16) p.pv.i);
261 n += irda_param_unpack(buf+2, "i", &p.pv.i);
262 if (type & PV_BIG_ENDIAN)
263 be32_to_cpus(&p.pv.i);
265 le32_to_cpus(&p.pv.i);
268 WARNING("%s: length %d not supported\n", __FUNCTION__, p.pl);
274 IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d, pi=%d\n", __FUNCTION__,
276 /* Call handler for this parameter */
277 err = (*func)(self, &p, PV_PUT);
281 return p.pl+2; /* Extracted pl+2 bytes */
285 * Function irda_extract_string (self, buf, len, type, func)
287 static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi,
288 PV_TYPE type, PI_HANDLER func)
294 IRDA_DEBUG(2, "%s()\n", __FUNCTION__);
296 p.pi = pi; /* In case handler needs to know */
297 p.pl = buf[1]; /* Extract lenght of value */
299 IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d\n", __FUNCTION__,
302 /* Check if buffer is long enough for parsing */
303 if (len < (2+p.pl)) {
304 WARNING("%s: buffer to short for parsing! "
305 "Need %d bytes, but len is only %d\n",
306 __FUNCTION__, p.pl, len);
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);
314 IRDA_DEBUG(2, "%s(), str=0x%02x 0x%02x\n", __FUNCTION__,
315 (__u8) str[0], (__u8) str[1]);
317 /* Null terminate string */
320 p.pv.c = str; /* Handler will need to take a copy */
322 /* Call handler for this parameter */
323 err = (*func)(self, &p, PV_PUT);
327 return p.pl+2; /* Extracted pl+2 bytes */
331 * Function irda_extract_octseq (self, buf, len, type, func)
333 static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
334 PV_TYPE type, PI_HANDLER func)
338 p.pi = pi; /* In case handler needs to know */
339 p.pl = buf[1]; /* Extract lenght of value */
341 /* Check if buffer is long enough for parsing */
342 if (len < (2+p.pl)) {
343 WARNING("%s: buffer to short for parsing! "
344 "Need %d bytes, but len is only %d\n",
345 __FUNCTION__, p.pl, len);
349 IRDA_DEBUG(0, "%s(), not impl\n", __FUNCTION__);
351 return p.pl+2; /* Extracted pl+2 bytes */
355 * Function irda_param_pack (skb, fmt, ...)
358 * 'i' = 32 bits integer
362 int irda_param_pack(__u8 *buf, char *fmt, ...)
371 for (p = fmt; *p != '\0'; p++) {
373 case 'b': /* 8 bits unsigned byte */
374 buf[n++] = (__u8)va_arg(args, int);
376 case 's': /* 16 bits unsigned short */
377 arg.i = (__u16)va_arg(args, int);
378 put_unaligned((__u16)arg.i, (__u16 *)(buf+n)); n+=2;
380 case 'i': /* 32 bits unsigned integer */
381 arg.i = va_arg(args, __u32);
382 put_unaligned(arg.i, (__u32 *)(buf+n)); n+=4;
385 case 'c': /* \0 terminated string */
386 arg.c = va_arg(args, char *);
387 strcpy(buf+n, arg.c);
388 n += strlen(arg.c) + 1;
400 EXPORT_SYMBOL(irda_param_pack);
403 * Function irda_param_unpack (skb, fmt, ...)
405 static int irda_param_unpack(__u8 *buf, char *fmt, ...)
414 for (p = fmt; *p != '\0'; p++) {
416 case 'b': /* 8 bits byte */
417 arg.ip = va_arg(args, __u32 *);
420 case 's': /* 16 bits short */
421 arg.ip = va_arg(args, __u32 *);
422 *arg.ip = get_unaligned((__u16 *)(buf+n)); n+=2;
424 case 'i': /* 32 bits unsigned integer */
425 arg.ip = va_arg(args, __u32 *);
426 *arg.ip = get_unaligned((__u32 *)(buf+n)); n+=4;
429 case 'c': /* \0 terminated string */
430 arg.c = va_arg(args, char *);
431 strcpy(arg.c, buf+n);
432 n += strlen(arg.c) + 1;
447 * Function irda_param_insert (self, pi, buf, len, info)
449 * Insert the specified parameter (pi) into buffer. Returns number of
452 int irda_param_insert(void *self, __u8 pi, __u8 *buf, int len,
453 pi_param_info_t *info)
455 pi_minor_info_t *pi_minor_info;
462 ASSERT(buf != NULL, return ret;);
463 ASSERT(info != 0, return ret;);
465 pi_minor = pi & info->pi_mask;
466 pi_major = pi >> info->pi_major_offset;
468 /* Check if the identifier value (pi) is valid */
469 if ((pi_major > info->len-1) ||
470 (pi_minor > info->tables[pi_major].len-1))
472 IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n",
475 /* Skip this parameter */
479 /* Lookup the info on how to parse this parameter */
480 pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
482 /* Find expected data type for this parameter identifier (pi)*/
483 type = pi_minor_info->type;
485 /* Check if handler has been implemented */
486 if (!pi_minor_info->func) {
487 MESSAGE("%s: no handler for pi=%#x\n", __FUNCTION__, pi);
488 /* Skip this parameter */
492 /* Insert parameter value */
493 ret = (*pv_insert_table[type & PV_MASK])(self, buf+n, len, pi, type,
494 pi_minor_info->func);
497 EXPORT_SYMBOL(irda_param_insert);
500 * Function irda_param_extract (self, buf, len, info)
502 * Parse all parameters. If len is correct, then everything should be
503 * safe. Returns the number of bytes that was parsed
506 static int irda_param_extract(void *self, __u8 *buf, int len,
507 pi_param_info_t *info)
509 pi_minor_info_t *pi_minor_info;
516 ASSERT(buf != NULL, return ret;);
517 ASSERT(info != 0, return ret;);
519 pi_minor = buf[n] & info->pi_mask;
520 pi_major = buf[n] >> info->pi_major_offset;
522 /* Check if the identifier value (pi) is valid */
523 if ((pi_major > info->len-1) ||
524 (pi_minor > info->tables[pi_major].len-1))
526 IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n",
527 __FUNCTION__, buf[0]);
529 /* Skip this parameter */
530 return 2 + buf[n + 1]; /* Continue */
533 /* Lookup the info on how to parse this parameter */
534 pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
536 /* Find expected data type for this parameter identifier (pi)*/
537 type = pi_minor_info->type;
539 IRDA_DEBUG(3, "%s(), pi=[%d,%d], type=%d\n", __FUNCTION__,
540 pi_major, pi_minor, type);
542 /* Check if handler has been implemented */
543 if (!pi_minor_info->func) {
544 MESSAGE("%s: no handler for pi=%#x\n", __FUNCTION__, buf[n]);
545 /* Skip this parameter */
546 return 2 + buf[n + 1]; /* Continue */
549 /* Parse parameter value */
550 ret = (*pv_extract_table[type & PV_MASK])(self, buf+n, len, buf[n],
551 type, pi_minor_info->func);
556 * Function irda_param_extract_all (self, buf, len, info)
558 * Parse all parameters. If len is correct, then everything should be
559 * safe. Returns the number of bytes that was parsed
562 int irda_param_extract_all(void *self, __u8 *buf, int len,
563 pi_param_info_t *info)
568 ASSERT(buf != NULL, return ret;);
569 ASSERT(info != 0, return ret;);
572 * Parse all parameters. Each parameter must be at least two bytes
573 * long or else there is no point in trying to parse it
576 ret = irda_param_extract(self, buf+n, len, info);
585 EXPORT_SYMBOL(irda_param_extract_all);