/*********************************************************************
|
/*********************************************************************
|
*
|
*
|
* Filename: parameters.c
|
* Filename: parameters.c
|
* Version: 1.0
|
* Version: 1.0
|
* Description: A more general way to handle (pi,pl,pv) parameters
|
* Description: A more general way to handle (pi,pl,pv) parameters
|
* Status: Experimental.
|
* Status: Experimental.
|
* Author: Dag Brattli <dagb@cs.uit.no>
|
* Author: Dag Brattli <dagb@cs.uit.no>
|
* Created at: Mon Jun 7 10:25:11 1999
|
* Created at: Mon Jun 7 10:25:11 1999
|
* Modified at: Sun Jan 30 14:08:39 2000
|
* Modified at: Sun Jan 30 14:08:39 2000
|
* Modified by: Dag Brattli <dagb@cs.uit.no>
|
* Modified by: Dag Brattli <dagb@cs.uit.no>
|
*
|
*
|
* Copyright (c) 1999-2000 Dag Brattli, All Rights Reserved.
|
* Copyright (c) 1999-2000 Dag Brattli, All Rights Reserved.
|
*
|
*
|
* This program is free software; you can redistribute it and/or
|
* This program is free software; you can redistribute it and/or
|
* modify it under the terms of the GNU General Public License as
|
* modify it under the terms of the GNU General Public License as
|
* published by the Free Software Foundation; either version 2 of
|
* published by the Free Software Foundation; either version 2 of
|
* the License, or (at your option) any later version.
|
* the License, or (at your option) any later version.
|
*
|
*
|
* This program is distributed in the hope that it will be useful,
|
* This program is distributed in the hope that it will be useful,
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
* GNU General Public License for more details.
|
* GNU General Public License for more details.
|
*
|
*
|
* You should have received a copy of the GNU General Public License
|
* You should have received a copy of the GNU General Public License
|
* along with this program; if not, write to the Free Software
|
* along with this program; if not, write to the Free Software
|
* Foundation, Inc., 59 Temple Place, Suite 330, Boston,
|
* Foundation, Inc., 59 Temple Place, Suite 330, Boston,
|
* MA 02111-1307 USA
|
* MA 02111-1307 USA
|
*
|
*
|
********************************************************************/
|
********************************************************************/
|
|
|
#include <linux/types.h>
|
#include <linux/types.h>
|
#include <asm/unaligned.h>
|
#include <asm/unaligned.h>
|
#include <asm/byteorder.h>
|
#include <asm/byteorder.h>
|
|
|
#include <net/irda/irda.h>
|
#include <net/irda/irda.h>
|
#include <net/irda/parameters.h>
|
#include <net/irda/parameters.h>
|
|
|
static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi,
|
static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi,
|
PV_TYPE type, PI_HANDLER func);
|
PV_TYPE type, PI_HANDLER func);
|
static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi,
|
static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi,
|
PV_TYPE type, PI_HANDLER func);
|
PV_TYPE type, PI_HANDLER func);
|
static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
|
static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
|
PV_TYPE type, PI_HANDLER func);
|
PV_TYPE type, PI_HANDLER func);
|
static int irda_extract_no_value(void *self, __u8 *buf, int len, __u8 pi,
|
static int irda_extract_no_value(void *self, __u8 *buf, int len, __u8 pi,
|
PV_TYPE type, PI_HANDLER func);
|
PV_TYPE type, PI_HANDLER func);
|
|
|
static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi,
|
static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi,
|
PV_TYPE type, PI_HANDLER func);
|
PV_TYPE type, PI_HANDLER func);
|
static int irda_insert_no_value(void *self, __u8 *buf, int len, __u8 pi,
|
static int irda_insert_no_value(void *self, __u8 *buf, int len, __u8 pi,
|
PV_TYPE type, PI_HANDLER func);
|
PV_TYPE type, PI_HANDLER func);
|
|
|
/* Parameter value call table. Must match PV_TYPE */
|
/* Parameter value call table. Must match PV_TYPE */
|
static PV_HANDLER pv_extract_table[] = {
|
static PV_HANDLER pv_extract_table[] = {
|
irda_extract_integer, /* Handler for any length integers */
|
irda_extract_integer, /* Handler for any length integers */
|
irda_extract_integer, /* Handler for 8 bits integers */
|
irda_extract_integer, /* Handler for 8 bits integers */
|
irda_extract_integer, /* Handler for 16 bits integers */
|
irda_extract_integer, /* Handler for 16 bits integers */
|
irda_extract_string, /* Handler for strings */
|
irda_extract_string, /* Handler for strings */
|
irda_extract_integer, /* Handler for 32 bits integers */
|
irda_extract_integer, /* Handler for 32 bits integers */
|
irda_extract_octseq, /* Handler for octet sequences */
|
irda_extract_octseq, /* Handler for octet sequences */
|
irda_extract_no_value /* Handler for no value parameters */
|
irda_extract_no_value /* Handler for no value parameters */
|
};
|
};
|
|
|
static PV_HANDLER pv_insert_table[] = {
|
static PV_HANDLER pv_insert_table[] = {
|
irda_insert_integer, /* Handler for any length integers */
|
irda_insert_integer, /* Handler for any length integers */
|
irda_insert_integer, /* Handler for 8 bits integers */
|
irda_insert_integer, /* Handler for 8 bits integers */
|
irda_insert_integer, /* Handler for 16 bits integers */
|
irda_insert_integer, /* Handler for 16 bits integers */
|
NULL, /* Handler for strings */
|
NULL, /* Handler for strings */
|
irda_insert_integer, /* Handler for 32 bits integers */
|
irda_insert_integer, /* Handler for 32 bits integers */
|
NULL, /* Handler for octet sequences */
|
NULL, /* Handler for octet sequences */
|
irda_insert_no_value /* Handler for no value parameters */
|
irda_insert_no_value /* Handler for no value parameters */
|
};
|
};
|
|
|
/*
|
/*
|
* Function irda_insert_no_value (self, buf, len, pi, type, func)
|
* Function irda_insert_no_value (self, buf, len, pi, type, func)
|
*
|
*
|
*
|
*
|
*
|
*
|
*/
|
*/
|
static int irda_insert_no_value(void *self, __u8 *buf, int len, __u8 pi,
|
static int irda_insert_no_value(void *self, __u8 *buf, int len, __u8 pi,
|
PV_TYPE type, PI_HANDLER func)
|
PV_TYPE type, PI_HANDLER func)
|
{
|
{
|
irda_param_t p;
|
irda_param_t p;
|
int ret;
|
int ret;
|
|
|
p.pi = pi;
|
p.pi = pi;
|
p.pl = 0;
|
p.pl = 0;
|
|
|
/* Call handler for this parameter */
|
/* Call handler for this parameter */
|
ret = (*func)(self, &p, PV_GET);
|
ret = (*func)(self, &p, PV_GET);
|
|
|
/* Extract values anyway, since handler may need them */
|
/* Extract values anyway, since handler may need them */
|
irda_param_pack(buf, "bb", p.pi, p.pl);
|
irda_param_pack(buf, "bb", p.pi, p.pl);
|
|
|
if (ret < 0)
|
if (ret < 0)
|
return ret;
|
return ret;
|
|
|
return 2; /* Inserted pl+2 bytes */
|
return 2; /* Inserted pl+2 bytes */
|
}
|
}
|
|
|
/*
|
/*
|
* Function irda_extract_no_value (self, buf, len, type, func)
|
* Function irda_extract_no_value (self, buf, len, type, func)
|
*
|
*
|
* Extracts a parameter without a pv field (pl=0)
|
* Extracts a parameter without a pv field (pl=0)
|
*
|
*
|
*/
|
*/
|
static int irda_extract_no_value(void *self, __u8 *buf, int len, __u8 pi,
|
static int irda_extract_no_value(void *self, __u8 *buf, int len, __u8 pi,
|
PV_TYPE type, PI_HANDLER func)
|
PV_TYPE type, PI_HANDLER func)
|
{
|
{
|
irda_param_t p;
|
irda_param_t p;
|
int ret;
|
int ret;
|
|
|
/* Extract values anyway, since handler may need them */
|
/* Extract values anyway, since handler may need them */
|
irda_param_unpack(buf, "bb", &p.pi, &p.pl);
|
irda_param_unpack(buf, "bb", &p.pi, &p.pl);
|
|
|
/* Call handler for this parameter */
|
/* Call handler for this parameter */
|
ret = (*func)(self, &p, PV_PUT);
|
ret = (*func)(self, &p, PV_PUT);
|
|
|
if (ret < 0)
|
if (ret < 0)
|
return ret;
|
return ret;
|
|
|
return 2; /* Extracted pl+2 bytes */
|
return 2; /* Extracted pl+2 bytes */
|
}
|
}
|
|
|
/*
|
/*
|
* Function irda_insert_integer (self, buf, len, pi, type, func)
|
* Function irda_insert_integer (self, buf, len, pi, type, func)
|
*
|
*
|
*
|
*
|
*
|
*
|
*/
|
*/
|
static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi,
|
static int irda_insert_integer(void *self, __u8 *buf, int len, __u8 pi,
|
PV_TYPE type, PI_HANDLER func)
|
PV_TYPE type, PI_HANDLER func)
|
{
|
{
|
irda_param_t p;
|
irda_param_t p;
|
int n = 0;
|
int n = 0;
|
int err;
|
int err;
|
|
|
p.pi = pi; /* In case handler needs to know */
|
p.pi = pi; /* In case handler needs to know */
|
p.pl = type & PV_MASK; /* The integer type codes the lenght as well */
|
p.pl = type & PV_MASK; /* The integer type codes the lenght as well */
|
p.pv.i = 0; /* Clear value */
|
p.pv.i = 0; /* Clear value */
|
|
|
/* Call handler for this parameter */
|
/* Call handler for this parameter */
|
err = (*func)(self, &p, PV_GET);
|
err = (*func)(self, &p, PV_GET);
|
if (err < 0)
|
if (err < 0)
|
return err;
|
return err;
|
|
|
/*
|
/*
|
* If parameter lenght is still 0, then (1) this is an any length
|
* If parameter lenght is still 0, then (1) this is an any length
|
* integer, and (2) the handler function does not care which length
|
* integer, and (2) the handler function does not care which length
|
* we choose to use, so we pick the one the gives the fewest bytes.
|
* we choose to use, so we pick the one the gives the fewest bytes.
|
*/
|
*/
|
if (p.pl == 0) {
|
if (p.pl == 0) {
|
if (p.pv.i < 0xff) {
|
if (p.pv.i < 0xff) {
|
IRDA_DEBUG(2, "%s(), using 1 byte\n", __FUNCTION__);
|
IRDA_DEBUG(2, "%s(), using 1 byte\n", __FUNCTION__);
|
p.pl = 1;
|
p.pl = 1;
|
} else if (p.pv.i < 0xffff) {
|
} else if (p.pv.i < 0xffff) {
|
IRDA_DEBUG(2, "%s(), using 2 bytes\n", __FUNCTION__);
|
IRDA_DEBUG(2, "%s(), using 2 bytes\n", __FUNCTION__);
|
p.pl = 2;
|
p.pl = 2;
|
} else {
|
} else {
|
IRDA_DEBUG(2, "%s(), using 4 bytes\n", __FUNCTION__);
|
IRDA_DEBUG(2, "%s(), using 4 bytes\n", __FUNCTION__);
|
p.pl = 4; /* Default length */
|
p.pl = 4; /* Default length */
|
}
|
}
|
}
|
}
|
/* Check if buffer is long enough for insertion */
|
/* Check if buffer is long enough for insertion */
|
if (len < (2+p.pl)) {
|
if (len < (2+p.pl)) {
|
WARNING("%s(), buffer to short for insertion!\n", __FUNCTION__);
|
WARNING("%s(), buffer to short for insertion!\n", __FUNCTION__);
|
return -1;
|
return -1;
|
}
|
}
|
IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d, pi=%d\n", __FUNCTION__, p.pi, p.pl, p.pv.i);
|
IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d, pi=%d\n", __FUNCTION__, p.pi, p.pl, p.pv.i);
|
switch (p.pl) {
|
switch (p.pl) {
|
case 1:
|
case 1:
|
n += irda_param_pack(buf, "bbb", p.pi, p.pl, (__u8) p.pv.i);
|
n += irda_param_pack(buf, "bbb", p.pi, p.pl, (__u8) p.pv.i);
|
break;
|
break;
|
case 2:
|
case 2:
|
if (type & PV_BIG_ENDIAN)
|
if (type & PV_BIG_ENDIAN)
|
p.pv.i = cpu_to_be16((__u16) p.pv.i);
|
p.pv.i = cpu_to_be16((__u16) p.pv.i);
|
else
|
else
|
p.pv.i = cpu_to_le16((__u16) p.pv.i);
|
p.pv.i = cpu_to_le16((__u16) p.pv.i);
|
n += irda_param_pack(buf, "bbs", p.pi, p.pl, (__u16) p.pv.i);
|
n += irda_param_pack(buf, "bbs", p.pi, p.pl, (__u16) p.pv.i);
|
break;
|
break;
|
case 4:
|
case 4:
|
if (type & PV_BIG_ENDIAN)
|
if (type & PV_BIG_ENDIAN)
|
cpu_to_be32s(&p.pv.i);
|
cpu_to_be32s(&p.pv.i);
|
else
|
else
|
cpu_to_le32s(&p.pv.i);
|
cpu_to_le32s(&p.pv.i);
|
n += irda_param_pack(buf, "bbi", p.pi, p.pl, p.pv.i);
|
n += irda_param_pack(buf, "bbi", p.pi, p.pl, p.pv.i);
|
|
|
break;
|
break;
|
default:
|
default:
|
WARNING("%s() length %d not supported\n", __FUNCTION__, p.pl);
|
WARNING("%s() length %d not supported\n", __FUNCTION__, p.pl);
|
/* Skip parameter */
|
/* Skip parameter */
|
return -1;
|
return -1;
|
}
|
}
|
|
|
return p.pl+2; /* Inserted pl+2 bytes */
|
return p.pl+2; /* Inserted pl+2 bytes */
|
}
|
}
|
|
|
/*
|
/*
|
* Function irda_extract integer (self, buf, len, pi, type, func)
|
* Function irda_extract integer (self, buf, len, pi, type, func)
|
*
|
*
|
* Extract a possibly variable length integer from buffer, and call
|
* Extract a possibly variable length integer from buffer, and call
|
* handler for processing of the parameter
|
* handler for processing of the parameter
|
*/
|
*/
|
static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi,
|
static int irda_extract_integer(void *self, __u8 *buf, int len, __u8 pi,
|
PV_TYPE type, PI_HANDLER func)
|
PV_TYPE type, PI_HANDLER func)
|
{
|
{
|
irda_param_t p;
|
irda_param_t p;
|
int n = 0;
|
int n = 0;
|
int extract_len; /* Real lenght we extract */
|
int extract_len; /* Real lenght we extract */
|
int err;
|
int err;
|
|
|
p.pi = pi; /* In case handler needs to know */
|
p.pi = pi; /* In case handler needs to know */
|
p.pl = buf[1]; /* Extract lenght of value */
|
p.pl = buf[1]; /* Extract lenght of value */
|
p.pv.i = 0; /* Clear value */
|
p.pv.i = 0; /* Clear value */
|
extract_len = p.pl; /* Default : extract all */
|
extract_len = p.pl; /* Default : extract all */
|
|
|
/* Check if buffer is long enough for parsing */
|
/* Check if buffer is long enough for parsing */
|
if (len < (2+p.pl)) {
|
if (len < (2+p.pl)) {
|
WARNING("%s(), buffer to short for parsing! "
|
WARNING("%s(), buffer to short for parsing! "
|
"Need %d bytes, but len is only %d\n", __FUNCTION__, p.pl, len);
|
"Need %d bytes, but len is only %d\n", __FUNCTION__, p.pl, len);
|
return -1;
|
return -1;
|
}
|
}
|
|
|
/*
|
/*
|
* Check that the integer length is what we expect it to be. If the
|
* Check that the integer length is what we expect it to be. If the
|
* handler want a 16 bits integer then a 32 bits is not good enough
|
* handler want a 16 bits integer then a 32 bits is not good enough
|
* PV_INTEGER means that the handler is flexible.
|
* PV_INTEGER means that the handler is flexible.
|
*/
|
*/
|
if (((type & PV_MASK) != PV_INTEGER) && ((type & PV_MASK) != p.pl)) {
|
if (((type & PV_MASK) != PV_INTEGER) && ((type & PV_MASK) != p.pl)) {
|
ERROR("%s(), invalid parameter length! "
|
ERROR("%s(), invalid parameter length! "
|
"Expected %d bytes, but value had %d bytes!\n",
|
"Expected %d bytes, but value had %d bytes!\n",
|
__FUNCTION__, type & PV_MASK, p.pl);
|
__FUNCTION__, type & PV_MASK, p.pl);
|
|
|
/* Most parameters are bit/byte fields or little endian,
|
/* Most parameters are bit/byte fields or little endian,
|
* so it's ok to only extract a subset of it (the subset
|
* so it's ok to only extract a subset of it (the subset
|
* that the handler expect). This is necessary, as some
|
* that the handler expect). This is necessary, as some
|
* broken implementations seems to add extra undefined bits.
|
* broken implementations seems to add extra undefined bits.
|
* If the parameter is shorter than we expect or is big
|
* If the parameter is shorter than we expect or is big
|
* endian, we can't play those tricks. Jean II */
|
* endian, we can't play those tricks. Jean II */
|
if((p.pl < (type & PV_MASK)) || (type & PV_BIG_ENDIAN)) {
|
if((p.pl < (type & PV_MASK)) || (type & PV_BIG_ENDIAN)) {
|
/* Skip parameter */
|
/* Skip parameter */
|
return p.pl+2;
|
return p.pl+2;
|
} else {
|
} else {
|
/* Extract subset of it, fallthrough */
|
/* Extract subset of it, fallthrough */
|
extract_len = type & PV_MASK;
|
extract_len = type & PV_MASK;
|
}
|
}
|
}
|
}
|
|
|
|
|
switch (extract_len) {
|
switch (extract_len) {
|
case 1:
|
case 1:
|
n += irda_param_unpack(buf+2, "b", &p.pv.i);
|
n += irda_param_unpack(buf+2, "b", &p.pv.i);
|
break;
|
break;
|
case 2:
|
case 2:
|
n += irda_param_unpack(buf+2, "s", &p.pv.i);
|
n += irda_param_unpack(buf+2, "s", &p.pv.i);
|
if (type & PV_BIG_ENDIAN)
|
if (type & PV_BIG_ENDIAN)
|
p.pv.i = be16_to_cpu((__u16) p.pv.i);
|
p.pv.i = be16_to_cpu((__u16) p.pv.i);
|
else
|
else
|
p.pv.i = le16_to_cpu((__u16) p.pv.i);
|
p.pv.i = le16_to_cpu((__u16) p.pv.i);
|
break;
|
break;
|
case 4:
|
case 4:
|
n += irda_param_unpack(buf+2, "i", &p.pv.i);
|
n += irda_param_unpack(buf+2, "i", &p.pv.i);
|
if (type & PV_BIG_ENDIAN)
|
if (type & PV_BIG_ENDIAN)
|
be32_to_cpus(&p.pv.i);
|
be32_to_cpus(&p.pv.i);
|
else
|
else
|
le32_to_cpus(&p.pv.i);
|
le32_to_cpus(&p.pv.i);
|
break;
|
break;
|
default:
|
default:
|
WARNING("%s() length %d not supported\n", __FUNCTION__, p.pl);
|
WARNING("%s() length %d not supported\n", __FUNCTION__, p.pl);
|
|
|
/* Skip parameter */
|
/* Skip parameter */
|
return p.pl+2;
|
return p.pl+2;
|
}
|
}
|
|
|
IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d, pi=%d\n", __FUNCTION__, p.pi, p.pl, p.pv.i);
|
IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d, pi=%d\n", __FUNCTION__, p.pi, p.pl, p.pv.i);
|
/* Call handler for this parameter */
|
/* Call handler for this parameter */
|
err = (*func)(self, &p, PV_PUT);
|
err = (*func)(self, &p, PV_PUT);
|
if (err < 0)
|
if (err < 0)
|
return err;
|
return err;
|
|
|
return p.pl+2; /* Extracted pl+2 bytes */
|
return p.pl+2; /* Extracted pl+2 bytes */
|
}
|
}
|
|
|
/*
|
/*
|
* Function irda_extract_string (self, buf, len, type, func)
|
* Function irda_extract_string (self, buf, len, type, func)
|
*
|
*
|
*
|
*
|
*
|
*
|
*/
|
*/
|
static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi,
|
static int irda_extract_string(void *self, __u8 *buf, int len, __u8 pi,
|
PV_TYPE type, PI_HANDLER func)
|
PV_TYPE type, PI_HANDLER func)
|
{
|
{
|
char str[33];
|
char str[33];
|
irda_param_t p;
|
irda_param_t p;
|
int err;
|
int err;
|
|
|
IRDA_DEBUG(2, "%s()\n", __FUNCTION__);
|
IRDA_DEBUG(2, "%s()\n", __FUNCTION__);
|
|
|
p.pi = pi; /* In case handler needs to know */
|
p.pi = pi; /* In case handler needs to know */
|
p.pl = buf[1]; /* Extract lenght of value */
|
p.pl = buf[1]; /* Extract lenght of value */
|
|
|
IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d\n", __FUNCTION__, p.pi, p.pl);
|
IRDA_DEBUG(2, "%s(), pi=%#x, pl=%d\n", __FUNCTION__, p.pi, p.pl);
|
|
|
/* Check if buffer is long enough for parsing */
|
/* Check if buffer is long enough for parsing */
|
if (len < (2+p.pl)) {
|
if (len < (2+p.pl)) {
|
WARNING("%s(), buffer to short for parsing! "
|
WARNING("%s(), buffer to short for parsing! "
|
"Need %d bytes, but len is only %d\n", __FUNCTION__, p.pl, len);
|
"Need %d bytes, but len is only %d\n", __FUNCTION__, p.pl, len);
|
return -1;
|
return -1;
|
}
|
}
|
|
|
/* Should be safe to copy string like this since we have already
|
/* Should be safe to copy string like this since we have already
|
* checked that the buffer is long enough */
|
* checked that the buffer is long enough */
|
strncpy(str, buf+2, p.pl);
|
strncpy(str, buf+2, p.pl);
|
|
|
IRDA_DEBUG(2, "%s(), str=0x%02x 0x%02x\n", __FUNCTION__, (__u8) str[0],
|
IRDA_DEBUG(2, "%s(), str=0x%02x 0x%02x\n", __FUNCTION__, (__u8) str[0],
|
(__u8) str[1]);
|
(__u8) str[1]);
|
|
|
/* Null terminate string */
|
/* Null terminate string */
|
str[p.pl+1] = '\0';
|
str[p.pl+1] = '\0';
|
|
|
p.pv.c = str; /* Handler will need to take a copy */
|
p.pv.c = str; /* Handler will need to take a copy */
|
|
|
/* Call handler for this parameter */
|
/* Call handler for this parameter */
|
err = (*func)(self, &p, PV_PUT);
|
err = (*func)(self, &p, PV_PUT);
|
if (err < 0)
|
if (err < 0)
|
return err;
|
return err;
|
|
|
return p.pl+2; /* Extracted pl+2 bytes */
|
return p.pl+2; /* Extracted pl+2 bytes */
|
}
|
}
|
|
|
/*
|
/*
|
* Function irda_extract_octseq (self, buf, len, type, func)
|
* Function irda_extract_octseq (self, buf, len, type, func)
|
*
|
*
|
*
|
*
|
*
|
*
|
*/
|
*/
|
static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
|
static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
|
PV_TYPE type, PI_HANDLER func)
|
PV_TYPE type, PI_HANDLER func)
|
{
|
{
|
irda_param_t p;
|
irda_param_t p;
|
|
|
p.pi = pi; /* In case handler needs to know */
|
p.pi = pi; /* In case handler needs to know */
|
p.pl = buf[1]; /* Extract lenght of value */
|
p.pl = buf[1]; /* Extract lenght of value */
|
|
|
/* Check if buffer is long enough for parsing */
|
/* Check if buffer is long enough for parsing */
|
if (len < (2+p.pl)) {
|
if (len < (2+p.pl)) {
|
WARNING("%s(), buffer to short for parsing! "
|
WARNING("%s(), buffer to short for parsing! "
|
"Need %d bytes, but len is only %d\n", __FUNCTION__, p.pl, len);
|
"Need %d bytes, but len is only %d\n", __FUNCTION__, p.pl, len);
|
return -1;
|
return -1;
|
}
|
}
|
|
|
IRDA_DEBUG(0, "%s(), not impl\n", __FUNCTION__);
|
IRDA_DEBUG(0, "%s(), not impl\n", __FUNCTION__);
|
|
|
return p.pl+2; /* Extracted pl+2 bytes */
|
return p.pl+2; /* Extracted pl+2 bytes */
|
}
|
}
|
|
|
/*
|
/*
|
* Function irda_param_pack (skb, fmt, ...)
|
* Function irda_param_pack (skb, fmt, ...)
|
*
|
*
|
* Format:
|
* Format:
|
* 'i' = 32 bits integer
|
* 'i' = 32 bits integer
|
* 's' = string
|
* 's' = string
|
*
|
*
|
*/
|
*/
|
int irda_param_pack(__u8 *buf, char *fmt, ...)
|
int irda_param_pack(__u8 *buf, char *fmt, ...)
|
{
|
{
|
irda_pv_t arg;
|
irda_pv_t arg;
|
va_list args;
|
va_list args;
|
char *p;
|
char *p;
|
int n = 0;
|
int n = 0;
|
|
|
va_start(args, fmt);
|
va_start(args, fmt);
|
|
|
for (p = fmt; *p != '\0'; p++) {
|
for (p = fmt; *p != '\0'; p++) {
|
switch (*p) {
|
switch (*p) {
|
case 'b': /* 8 bits unsigned byte */
|
case 'b': /* 8 bits unsigned byte */
|
buf[n++] = (__u8)va_arg(args, int);
|
buf[n++] = (__u8)va_arg(args, int);
|
break;
|
break;
|
case 's': /* 16 bits unsigned short */
|
case 's': /* 16 bits unsigned short */
|
arg.i = (__u16)va_arg(args, int);
|
arg.i = (__u16)va_arg(args, int);
|
put_unaligned((__u16)arg.i, (__u16 *)(buf+n)); n+=2;
|
put_unaligned((__u16)arg.i, (__u16 *)(buf+n)); n+=2;
|
break;
|
break;
|
case 'i': /* 32 bits unsigned integer */
|
case 'i': /* 32 bits unsigned integer */
|
arg.i = va_arg(args, __u32);
|
arg.i = va_arg(args, __u32);
|
put_unaligned(arg.i, (__u32 *)(buf+n)); n+=4;
|
put_unaligned(arg.i, (__u32 *)(buf+n)); n+=4;
|
break;
|
break;
|
#if 0
|
#if 0
|
case 'c': /* \0 terminated string */
|
case 'c': /* \0 terminated string */
|
arg.c = va_arg(args, char *);
|
arg.c = va_arg(args, char *);
|
strcpy(buf+n, arg.c);
|
strcpy(buf+n, arg.c);
|
n += strlen(arg.c) + 1;
|
n += strlen(arg.c) + 1;
|
break;
|
break;
|
#endif
|
#endif
|
default:
|
default:
|
va_end(args);
|
va_end(args);
|
return -1;
|
return -1;
|
}
|
}
|
|
|
}
|
}
|
va_end(args);
|
va_end(args);
|
|
|
return 0;
|
return 0;
|
}
|
}
|
|
|
/*
|
/*
|
* Function irda_param_unpack (skb, fmt, ...)
|
* Function irda_param_unpack (skb, fmt, ...)
|
*
|
*
|
*
|
*
|
*
|
*
|
*/
|
*/
|
int irda_param_unpack(__u8 *buf, char *fmt, ...)
|
int irda_param_unpack(__u8 *buf, char *fmt, ...)
|
{
|
{
|
irda_pv_t arg;
|
irda_pv_t arg;
|
va_list args;
|
va_list args;
|
char *p;
|
char *p;
|
int n = 0;
|
int n = 0;
|
|
|
va_start(args, fmt);
|
va_start(args, fmt);
|
|
|
for (p = fmt; *p != '\0'; p++) {
|
for (p = fmt; *p != '\0'; p++) {
|
switch (*p) {
|
switch (*p) {
|
case 'b': /* 8 bits byte */
|
case 'b': /* 8 bits byte */
|
arg.ip = va_arg(args, __u32 *);
|
arg.ip = va_arg(args, __u32 *);
|
*arg.ip = buf[n++];
|
*arg.ip = buf[n++];
|
break;
|
break;
|
case 's': /* 16 bits short */
|
case 's': /* 16 bits short */
|
arg.ip = va_arg(args, __u32 *);
|
arg.ip = va_arg(args, __u32 *);
|
*arg.ip = get_unaligned((__u16 *)(buf+n)); n+=2;
|
*arg.ip = get_unaligned((__u16 *)(buf+n)); n+=2;
|
break;
|
break;
|
case 'i': /* 32 bits unsigned integer */
|
case 'i': /* 32 bits unsigned integer */
|
arg.ip = va_arg(args, __u32 *);
|
arg.ip = va_arg(args, __u32 *);
|
*arg.ip = get_unaligned((__u32 *)(buf+n)); n+=4;
|
*arg.ip = get_unaligned((__u32 *)(buf+n)); n+=4;
|
break;
|
break;
|
#if 0
|
#if 0
|
case 'c': /* \0 terminated string */
|
case 'c': /* \0 terminated string */
|
arg.c = va_arg(args, char *);
|
arg.c = va_arg(args, char *);
|
strcpy(arg.c, buf+n);
|
strcpy(arg.c, buf+n);
|
n += strlen(arg.c) + 1;
|
n += strlen(arg.c) + 1;
|
break;
|
break;
|
#endif
|
#endif
|
default:
|
default:
|
va_end(args);
|
va_end(args);
|
return -1;
|
return -1;
|
}
|
}
|
|
|
}
|
}
|
va_end(args);
|
va_end(args);
|
|
|
return 0;
|
return 0;
|
}
|
}
|
|
|
/*
|
/*
|
* Function irda_param_insert (self, pi, buf, len, info)
|
* Function irda_param_insert (self, pi, buf, len, info)
|
*
|
*
|
* Insert the specified parameter (pi) into buffer. Returns number of
|
* Insert the specified parameter (pi) into buffer. Returns number of
|
* bytes inserted
|
* bytes inserted
|
*/
|
*/
|
int irda_param_insert(void *self, __u8 pi, __u8 *buf, int len,
|
int irda_param_insert(void *self, __u8 pi, __u8 *buf, int len,
|
pi_param_info_t *info)
|
pi_param_info_t *info)
|
{
|
{
|
pi_minor_info_t *pi_minor_info;
|
pi_minor_info_t *pi_minor_info;
|
__u8 pi_minor;
|
__u8 pi_minor;
|
__u8 pi_major;
|
__u8 pi_major;
|
int type;
|
int type;
|
int ret = -1;
|
int ret = -1;
|
int n = 0;
|
int n = 0;
|
|
|
ASSERT(buf != NULL, return ret;);
|
ASSERT(buf != NULL, return ret;);
|
ASSERT(info != 0, return ret;);
|
ASSERT(info != 0, return ret;);
|
|
|
pi_minor = pi & info->pi_mask;
|
pi_minor = pi & info->pi_mask;
|
pi_major = pi >> info->pi_major_offset;
|
pi_major = pi >> info->pi_major_offset;
|
|
|
/* Check if the identifier value (pi) is valid */
|
/* Check if the identifier value (pi) is valid */
|
if ((pi_major > info->len-1) ||
|
if ((pi_major > info->len-1) ||
|
(pi_minor > info->tables[pi_major].len-1))
|
(pi_minor > info->tables[pi_major].len-1))
|
{
|
{
|
IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n", __FUNCTION__, pi);
|
IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n", __FUNCTION__, pi);
|
|
|
/* Skip this parameter */
|
/* Skip this parameter */
|
return -1;
|
return -1;
|
}
|
}
|
|
|
/* Lookup the info on how to parse this parameter */
|
/* Lookup the info on how to parse this parameter */
|
pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
|
pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
|
|
|
/* Find expected data type for this parameter identifier (pi)*/
|
/* Find expected data type for this parameter identifier (pi)*/
|
type = pi_minor_info->type;
|
type = pi_minor_info->type;
|
|
|
/* Check if handler has been implemented */
|
/* Check if handler has been implemented */
|
if (!pi_minor_info->func) {
|
if (!pi_minor_info->func) {
|
MESSAGE("%s(), no handler for pi=%#x\n", __FUNCTION__, pi);
|
MESSAGE("%s(), no handler for pi=%#x\n", __FUNCTION__, pi);
|
/* Skip this parameter */
|
/* Skip this parameter */
|
return -1;
|
return -1;
|
}
|
}
|
|
|
/* Insert parameter value */
|
/* Insert parameter value */
|
ret = (*pv_insert_table[type & PV_MASK])(self, buf+n, len, pi, type,
|
ret = (*pv_insert_table[type & PV_MASK])(self, buf+n, len, pi, type,
|
pi_minor_info->func);
|
pi_minor_info->func);
|
return ret;
|
return ret;
|
}
|
}
|
|
|
/*
|
/*
|
* Function irda_param_extract_all (self, buf, len, info)
|
* Function irda_param_extract_all (self, buf, len, info)
|
*
|
*
|
* Parse all parameters. If len is correct, then everything should be
|
* Parse all parameters. If len is correct, then everything should be
|
* safe. Returns the number of bytes that was parsed
|
* safe. Returns the number of bytes that was parsed
|
*
|
*
|
*/
|
*/
|
int irda_param_extract(void *self, __u8 *buf, int len, pi_param_info_t *info)
|
int irda_param_extract(void *self, __u8 *buf, int len, pi_param_info_t *info)
|
{
|
{
|
pi_minor_info_t *pi_minor_info;
|
pi_minor_info_t *pi_minor_info;
|
__u8 pi_minor;
|
__u8 pi_minor;
|
__u8 pi_major;
|
__u8 pi_major;
|
int type;
|
int type;
|
int ret = -1;
|
int ret = -1;
|
int n = 0;
|
int n = 0;
|
|
|
ASSERT(buf != NULL, return ret;);
|
ASSERT(buf != NULL, return ret;);
|
ASSERT(info != 0, return ret;);
|
ASSERT(info != 0, return ret;);
|
|
|
pi_minor = buf[n] & info->pi_mask;
|
pi_minor = buf[n] & info->pi_mask;
|
pi_major = buf[n] >> info->pi_major_offset;
|
pi_major = buf[n] >> info->pi_major_offset;
|
|
|
/* Check if the identifier value (pi) is valid */
|
/* Check if the identifier value (pi) is valid */
|
if ((pi_major > info->len-1) ||
|
if ((pi_major > info->len-1) ||
|
(pi_minor > info->tables[pi_major].len-1))
|
(pi_minor > info->tables[pi_major].len-1))
|
{
|
{
|
IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n", __FUNCTION__,
|
IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n", __FUNCTION__,
|
buf[0]);
|
buf[0]);
|
|
|
/* Skip this parameter */
|
/* Skip this parameter */
|
return 2 + buf[n + 1]; /* Continue */
|
return 2 + buf[n + 1]; /* Continue */
|
}
|
}
|
|
|
/* Lookup the info on how to parse this parameter */
|
/* Lookup the info on how to parse this parameter */
|
pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
|
pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
|
|
|
/* Find expected data type for this parameter identifier (pi)*/
|
/* Find expected data type for this parameter identifier (pi)*/
|
type = pi_minor_info->type;
|
type = pi_minor_info->type;
|
|
|
IRDA_DEBUG(3, "%s(), pi=[%d,%d], type=%d\n", __FUNCTION__,
|
IRDA_DEBUG(3, "%s(), pi=[%d,%d], type=%d\n", __FUNCTION__,
|
pi_major, pi_minor, type);
|
pi_major, pi_minor, type);
|
|
|
/* Check if handler has been implemented */
|
/* Check if handler has been implemented */
|
if (!pi_minor_info->func) {
|
if (!pi_minor_info->func) {
|
MESSAGE("%s(), no handler for pi=%#x\n", __FUNCTION__, buf[n]);
|
MESSAGE("%s(), no handler for pi=%#x\n", __FUNCTION__, buf[n]);
|
/* Skip this parameter */
|
/* Skip this parameter */
|
return 2 + buf[n + 1]; /* Continue */
|
return 2 + buf[n + 1]; /* Continue */
|
}
|
}
|
|
|
/* Parse parameter value */
|
/* Parse parameter value */
|
ret = (*pv_extract_table[type & PV_MASK])(self, buf+n, len, buf[n],
|
ret = (*pv_extract_table[type & PV_MASK])(self, buf+n, len, buf[n],
|
type, pi_minor_info->func);
|
type, pi_minor_info->func);
|
return ret;
|
return ret;
|
}
|
}
|
|
|
/*
|
/*
|
* Function irda_param_extract_all (self, buf, len, info)
|
* Function irda_param_extract_all (self, buf, len, info)
|
*
|
*
|
* Parse all parameters. If len is correct, then everything should be
|
* Parse all parameters. If len is correct, then everything should be
|
* safe. Returns the number of bytes that was parsed
|
* safe. Returns the number of bytes that was parsed
|
*
|
*
|
*/
|
*/
|
int irda_param_extract_all(void *self, __u8 *buf, int len,
|
int irda_param_extract_all(void *self, __u8 *buf, int len,
|
pi_param_info_t *info)
|
pi_param_info_t *info)
|
{
|
{
|
int ret = -1;
|
int ret = -1;
|
int n = 0;
|
int n = 0;
|
|
|
ASSERT(buf != NULL, return ret;);
|
ASSERT(buf != NULL, return ret;);
|
ASSERT(info != 0, return ret;);
|
ASSERT(info != 0, return ret;);
|
|
|
/*
|
/*
|
* Parse all parameters. Each parameter must be at least two bytes
|
* Parse all parameters. Each parameter must be at least two bytes
|
* long or else there is no point in trying to parse it
|
* long or else there is no point in trying to parse it
|
*/
|
*/
|
while (len > 2) {
|
while (len > 2) {
|
ret = irda_param_extract(self, buf+n, len, info);
|
ret = irda_param_extract(self, buf+n, len, info);
|
if (ret < 0)
|
if (ret < 0)
|
return ret;
|
return ret;
|
|
|
n += ret;
|
n += ret;
|
len -= ret;
|
len -= ret;
|
}
|
}
|
return n;
|
return n;
|
}
|
}
|
|
|
|
|
