parameters.c

sparc硬件平台上的红外协议

		p.pi, p.pl);
	
	/* Check if buffer is long enough for parsing */
	if (len < (2+p.pl)) {
		IRDA_WARNING("%s: buffer too short for parsing! "
			"Need %d bytes, but len is only %d\n",
			__FUNCTION__, p.pl, len);
		return -1;
	}
	
	/* Should be safe to copy string like this since we have already
	* checked that the buffer is long enough */
	strncpy(str, buf+2, p.pl);
	
	IRDA_DEBUG(2, "%s(), str=0x%02x 0x%02x\n", __FUNCTION__,
		(__u8) str[0], (__u8) str[1]);
	
	/* Null terminate string */
	str[p.pl+1] = '\0';
	
	p.pv.c = str; /* Handler will need to take a copy */
	
	/* Call handler for this parameter */
	err = (*func)(self, &p, PV_PUT);
	if (err < 0)
		return err;
	
	return p.pl+2; /* Extracted pl+2 bytes */
}

/*
* Function irda_extract_octseq (self, buf, len, type, func)
*/
static int irda_extract_octseq(void *self, __u8 *buf, int len, __u8 pi,
							   PV_TYPE type, PI_HANDLER func)
{
	irda_param_t p;
	
	p.pi = pi;     /* In case handler needs to know */
	p.pl = buf[1]; /* Extract lenght of value */
	
	/* Check if buffer is long enough for parsing */
	if (len < (2+p.pl)) {
		IRDA_WARNING("%s: buffer too short for parsing! "
			"Need %d bytes, but len is only %d\n",
			__FUNCTION__, p.pl, len);
		return -1;
	}
	
	IRDA_DEBUG(0, "%s(), not impl\n", __FUNCTION__);
	
	return p.pl+2; /* Extracted pl+2 bytes */
}

/*
* Function irda_param_pack (skb, fmt, ...)
*
*    Format:
*        'i' = 32 bits integer
*        's' = string
*
*/
int irda_param_pack(__u8 *buf, char *fmt, ...)
{
	irda_pv_t arg;
	va_list args;
	char *p;
	int n = 0;
	
	va_start(args, fmt);
	
	for (p = fmt; *p != '\0'; p++) {
		switch (*p) {
		case 'b':  /* 8 bits unsigned byte */
			buf[n++] = (__u8)va_arg(args, int);
			break;
		case 's':  /* 16 bits unsigned short */
			arg.i = (__u16)va_arg(args, int);
			//modify by  xugangan
			//put_unaligned((__u16)arg.i, (__u16 *)(buf+n)); 
			buf[n+1] = (__u8)arg.i;
			buf[n] = (__u8)(arg.i >> 8);
			n+=2;
			break;
		case 'i':  /* 32 bits unsigned integer */
			arg.i = va_arg(args, __u32);
			//modify by  xugangan
			//put_unaligned(arg.i, (__u32 *)(buf+n));
			buf[n+3] = (__u8)arg.i;
			buf[n+2] = (__u8)(arg.i >> 8);
			buf[n+1] = (__u8)(arg.i >> 16);
			buf[n] = (__u8)(arg.i >> 24);
			n+=4;
			break;
#if 0
		case 'c': /* \0 terminated string */
			arg.c = va_arg(args, char *);
			strcpy(buf+n, arg.c);
			n += strlen(arg.c) + 1;
			break;
#endif
		default:
			va_end(args);
			return -1;
		}
	}
	va_end(args);
	
	return 0;
}

/*
* Function irda_param_unpack (skb, fmt, ...)
*/
static int irda_param_unpack(__u8 *buf, char *fmt, ...)
{
	irda_pv_t arg;
	va_list args;
	char *p;
	int n = 0;
	
	va_start(args, fmt);
	
	for (p = fmt; *p != '\0'; p++) {
		switch (*p) {
		case 'b':  /* 8 bits byte */
			arg.ip = va_arg(args, __u32 *);
			*arg.ip = buf[n++];
			break;
		case 's':  /* 16 bits short */
			arg.ip = va_arg(args, __u32 *);
			//modify by xugangan
			
			//*arg.ip = get_unaligned((__u16 *)(buf+n)); 
			*arg.ip = ((__u16)(*(buf+n)))<<8 |(__u16)(*(buf+n+1)) ; 
			n+=2;
			break;
		case 'i':  /* 32 bits unsigned integer */
			arg.ip = va_arg(args, __u32 *);
			//*arg.ip = get_unaligned((__u32 *)(buf+n)); n+=4;
			
			*arg.ip = ((__u16)(buf[n]))<<24 |((__u16)(buf[n+1]))<<16 |((__u16)(buf[n+2]))<<8 |(__u16)(buf[n+3]) ; 
			break;
#if 0
		case 'c':   /* \0 terminated string */
			arg.c = va_arg(args, char *);
			strcpy(arg.c, buf+n);
			n += strlen(arg.c) + 1;
			break;
#endif
		default:
			va_end(args);
			return -1;
		}
		
	}
	va_end(args);
	
	return 0;
}

/*
* Function irda_param_insert (self, pi, buf, len, info)
*
*    Insert the specified parameter (pi) into buffer. Returns number of
*    bytes inserted
*/
int irda_param_insert(void *self, __u8 pi, __u8 *buf, int len,
					  pi_param_info_t *info)
{
	pi_minor_info_t *pi_minor_info;
	__u8 pi_minor;
	__u8 pi_major;
	int type;
	int ret = -1;
	int n = 0;
	
	IRDA_ASSERT(buf != NULL, return ret;);
	IRDA_ASSERT(info != 0, return ret;);
	
	pi_minor = pi & info->pi_mask;
	pi_major = pi >> info->pi_major_offset;
	
	/* Check if the identifier value (pi) is valid */
	if ((pi_major > info->len-1) ||
		(pi_minor > info->tables[pi_major].len-1))
	{
		IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n",
			__FUNCTION__, pi);
		
		/* Skip this parameter */
		return -1;
	}
	
	/* Lookup the info on how to parse this parameter */
	pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
	
	/* Find expected data type for this parameter identifier (pi)*/
	type = pi_minor_info->type;
	
	/*  Check if handler has been implemented */
	if (!pi_minor_info->func) {
		IRDA_MESSAGE("%s: no handler for pi=%#x\n", __FUNCTION__, pi);
		/* Skip this parameter */
		return -1;
	}
	
	/* Insert parameter value */
	ret = (*pv_insert_table[type & PV_MASK])(self, buf+n, len, pi, type,
		pi_minor_info->func);
	return ret;
}
/*
* Function irda_param_extract (self, buf, len, info)
*
*    Parse all parameters. If len is correct, then everything should be
*    safe. Returns the number of bytes that was parsed
*
*/
static int irda_param_extract(void *self, __u8 *buf, int len,
							  pi_param_info_t *info)
{
	pi_minor_info_t *pi_minor_info;
	__u8 pi_minor;
	__u8 pi_major;
	int type;
	int ret = -1;
	int n = 0;
	
	IRDA_ASSERT(buf != NULL, return ret;);
	IRDA_ASSERT(info != 0, return ret;);
	
	pi_minor = buf[n] & info->pi_mask;
	pi_major = buf[n] >> info->pi_major_offset;
	
	/* Check if the identifier value (pi) is valid */
	if ((pi_major > info->len-1) ||
		(pi_minor > info->tables[pi_major].len-1))
	{
		IRDA_DEBUG(0, "%s(), no handler for parameter=0x%02x\n",
			__FUNCTION__, buf[0]);
		
		/* Skip this parameter */
		return 2 + buf[n + 1];  /* Continue */
	}
	
	/* Lookup the info on how to parse this parameter */
	pi_minor_info = &info->tables[pi_major].pi_minor_call_table[pi_minor];
	
	/* Find expected data type for this parameter identifier (pi)*/
	type = pi_minor_info->type;
	
	IRDA_DEBUG(2, "%s(), pi=[%d,%d], type=%d\n", __FUNCTION__,
		pi_major, pi_minor, type);
	
	/*  Check if handler has been implemented */
	if (!pi_minor_info->func) {
		IRDA_MESSAGE("%s: no handler for pi=%#x\n",
			__FUNCTION__, buf[n]);
		/* Skip this parameter */
		return 2 + buf[n + 1]; /* Continue */
	}
	
	/* Parse parameter value */
	ret = (*pv_extract_table[type & PV_MASK])(self, buf+n, len, buf[n],
						  type, pi_minor_info->func);
	return ret;
}

/*
* Function irda_param_extract_all (self, buf, len, info)
*
*    Parse all parameters. If len is correct, then everything should be
*    safe. Returns the number of bytes that was parsed
*
*/
int irda_param_extract_all(void *self, __u8 *buf, int len,
						   pi_param_info_t *info)
{
	int ret = -1;
	int n = 0;
	
	IRDA_ASSERT(buf != NULL, return ret;);
	IRDA_ASSERT(info != 0, return ret;);
	
	/*
	* Parse all parameters. Each parameter must be at least two bytes
	* long or else there is no point in trying to parse it
	*/
	while (len > 2) {
		ret = irda_param_extract(self, buf+n, len, info);
		if (ret < 0)
			return ret;
		
		n += ret;
		len -= ret;
	}
	return n;
}