/*
 * CDDL HEADER START
 *
 * The contents of this file are subject to the terms of the
 * Common Development and Distribution License, Version 1.0 only
 * (the "License").  You may not use this file except in compliance
 * with the License.
 *
 * You can obtain a copy of the license at usr/src/OPENSOLARIS.LICENSE
 * or http://www.opensolaris.org/os/licensing.
 * See the License for the specific language governing permissions
 * and limitations under the License.
 *
 * When distributing Covered Code, include this CDDL HEADER in each
 * file and include the License file at usr/src/OPENSOLARIS.LICENSE.
 * If applicable, add the following below this CDDL HEADER, with the
 * fields enclosed by brackets "[]" replaced with your own identifying
 * information: Portions Copyright [yyyy] [name of copyright owner]
 *
 * CDDL HEADER END
 */
/*
 * Copyright 2004 Sun Microsystems, Inc.  All rights reserved.
 * Use is subject to license terms.
 */

#pragma ident	"%Z%%M%	%I%	%E% SMI"

/*
 * Descriptor parsing functions
 */

#include <sys/types.h>
#include <sys/systm.h>
#include <sys/inttypes.h>
#include <sys/ib/mgt/ibmf/ibmf_utils.h>
#include <sys/debug.h>

#define	INCREMENT_BUF(buf) \
		if ((buf)[0] == 0) { \
			break; \
		} else { \
			(buf) += (buf)[0]; \
		}
#define	isdigit(ch) ((ch >= '0') && (ch <= '9'))

/*
 * ibmf_utils_unpack_data:
 *
 * parser function which takes a format string, a void pointer, and a character
 * buffer and parses the buffer according to the identifiers in the format
 * string.  Copies the data from the buffer and places into the structure,
 * taking care of byte swapping and any padding due to 64-bit Solaris.  Modified
 * from /ws/on81-gate/usr/src/uts/common/io/usb/usba/parser.c.
 *
 * The data and structure length parameters can be larger than the number of
 * bytes specified in the format.  unpack_data will use the smallest of the
 * three values, stopping when it finishes parsing the format string or reaches
 * the end of one of the two buffers.
 */
void
ibmf_utils_unpack_data(char *format,
	uchar_t *data,
	size_t datalen,
	void *structure,
	size_t structlen)
{
	int	fmt;
	int	multiplier = 0;
	uchar_t	*dataend = data + datalen;
	char	*structstart = (char *)structure;
	void	*structend = (void *)((intptr_t)structstart + structlen);

	while ((fmt = *format) != '\0') {

		if (fmt == 'c') {
			uint8_t	*cp = (uint8_t *)structure;

			/*
			 * account for possible hole in structure
			 * due to unaligned data
			 */
			cp = (uint8_t *)
			    (((uintptr_t)cp + _CHAR_ALIGNMENT - 1) &
			    ~(_CHAR_ALIGNMENT - 1));

			if (((data + 1) > dataend) ||
			    ((cp + 1) > (uint8_t *)structend))
				break;

			*cp++ = *data++;
			structure = (void *)cp;
			if (multiplier) {
				multiplier--;
			}
			if (multiplier == 0) {
				format++;
			}
		} else if (fmt == 's') {
			uint16_t	*sp = (uint16_t *)structure;

			/*
			 * account for possible hole in structure
			 * due to unaligned data
			 */
			sp = (uint16_t *)
			    (((uintptr_t)sp + _SHORT_ALIGNMENT - 1) &
			    ~(_SHORT_ALIGNMENT - 1));

			if (((data + 2) > dataend) ||
			    ((sp + 1) > (uint16_t *)structend))
				break;

			*sp++ = (data[0] << 8) + data[1];
			data += 2;
			structure = (void *)sp;
			if (multiplier) {
				multiplier--;
			}
			if (multiplier == 0) {
				format++;
			}
		} else if (fmt == 'l') {
			uint32_t 	*lp = (uint32_t *)structure;

			/*
			 * account for possible hole in structure
			 * due to unaligned data
			 */
			lp = (uint32_t *)
			    (((uintptr_t)lp + _INT_ALIGNMENT - 1) &
			    ~(_INT_ALIGNMENT - 1));

			if (((data + 4) > dataend) ||
			    ((lp + 1) > (uint32_t *)structend))
				break;

			*lp++ = ((((((uint32_t)data[0] << 8) | data[1]) << 8)
			    | data[2]) << 8) | data[3];

			data += 4;
			structure = (void *)lp;
			if (multiplier) {
				multiplier--;
			}
			if (multiplier == 0) {
				format++;
			}
		} else if (fmt == 'L') {
			uint64_t	*llp = (uint64_t *)structure;

			/*
			 * account for possible hole in structure
			 * due to unaligned data
			 */
			llp = (uint64_t *)
			    (((uintptr_t)llp + _LONG_LONG_ALIGNMENT - 1) &
			    ~(_LONG_LONG_ALIGNMENT - 1));

			if (((data + 8) > dataend) ||
			    ((llp + 1) > (uint64_t *)structend))
				break;
			/*
			 * note: data[0] is cast to uint64_t so that the
			 * compiler wouldn't treat the results of the shifts
			 * as a 32bit quantity; we really want to get 64bits
			 * out of this.
			 */
			*llp++ = ((((((((((((((uint64_t)data[0] << 8) |
				data[1]) << 8) | data[2]) << 8) |
				data[3]) << 8) | data[4]) << 8) |
				data[5]) << 8) | data[6]) << 8) |
				data[7];

			data += 8;
			structure = (void *)llp;
			if (multiplier) {
				multiplier--;
			}
			if (multiplier == 0) {
				format++;
			}
		} else if (isdigit(fmt)) {
			multiplier = (multiplier * 10) + (fmt - '0');
			format++;
		} else {
			multiplier = 0;
			break;
		}
	}
}

/*
 * ibmf_utils_pack_data:
 *
 * parser function which takes a format string, a void pointer, and a character
 * buffer and parses the structure according to the identifiers in the format
 * string.  Copies the data from the structure and places in the buffer, taking
 * care of byte swapping and any padding due to 64-bit Solaris.  Modified from
 * /ws/on81-gate/usr/src/uts/common/io/usb/usba/parser.c.
 *
 */
void
ibmf_utils_pack_data(char *format, void *structure,
    size_t structlen, uchar_t *data, size_t datalen)
{
	int	fmt;
	int	multiplier = 0;
	uchar_t	*dataend = data + datalen;
	char	*structend = (void *)((uchar_t *)structure + structlen);

	while ((fmt = *format) != '\0') {
		if (fmt == 'c') {
			uint8_t	*cp = (uint8_t *)structure;

			/*
			 * account for possible hole in structure
			 * due to unaligned data
			 */
			cp = (uint8_t *)
			    (((uintptr_t)cp + _CHAR_ALIGNMENT - 1) &
			    ~(_CHAR_ALIGNMENT - 1));

			if (((data + 1) > dataend) ||
			    ((cp + 1) > (uint8_t *)structend)) {
				break;
			}

			*data++ = *cp++;
			structure = (void *)cp;
			if (multiplier) {
				multiplier--;
			}
			if (multiplier == 0) {
				format++;
			}
		} else if (fmt == 's') {
			uint16_t	*sp = (uint16_t *)structure;

			/*
			 * account for possible hole in structure
			 * due to unaligned data
			 */
			sp = (uint16_t *)
			    (((uintptr_t)sp + _SHORT_ALIGNMENT - 1) &
			    ~(_SHORT_ALIGNMENT - 1));

			if (((data + 2) > dataend) ||
			    ((sp + 1) > (uint16_t *)structend))
				break;

			/* do an endian-independent copy */
			data[0] = (uchar_t)(*sp >> 8);
			data[1] = (uchar_t)(*sp);

			sp++;
			data += 2;

			structure = (void *)sp;
			if (multiplier) {
				multiplier--;
			}
			if (multiplier == 0) {
				format++;
			}
		} else if (fmt == 'l') {
			uint32_t 	*lp = (uint32_t *)structure;

			/*
			 * account for possible hole in structure
			 * due to unaligned data
			 */
			lp = (uint32_t *)
			    (((uintptr_t)lp + _INT_ALIGNMENT - 1) &
			    ~(_INT_ALIGNMENT - 1));

			if (((data + 4) > dataend) ||
			    ((lp + 1) > (uint32_t *)structend))
				break;

			/* do an endian-independent copy */
			data[0] = (uchar_t)(*lp >> 24);
			data[1] = (uchar_t)(*lp >> 16);
			data[2] = (uchar_t)(*lp >> 8);
			data[3] = (uchar_t)(*lp);

			lp++;
			data += 4;

			structure = (void *)lp;
			if (multiplier) {
				multiplier--;
			}
			if (multiplier == 0) {
				format++;
			}
		} else if (fmt == 'L') {
			uint64_t	*llp = (uint64_t *)structure;

			/*
			 * account for possible hole in structure
			 * due to unaligned data
			 */
			llp = (uint64_t *)
			    (((uintptr_t)llp + _LONG_LONG_ALIGNMENT - 1) &
			    ~(_LONG_LONG_ALIGNMENT - 1));

			if (((data + 8) > dataend) ||
			    ((llp + 1) > (uint64_t *)structend))
				break;

			/* do an endian-independent copy */
			data[0] = (uchar_t)(*llp >> 56);
			data[1] = (uchar_t)(*llp >> 48);
			data[2] = (uchar_t)(*llp >> 40);
			data[3] = (uchar_t)(*llp >> 32);
			data[4] = (uchar_t)(*llp >> 24);
			data[5] = (uchar_t)(*llp >> 16);
			data[6] = (uchar_t)(*llp >> 8);
			data[7] = (uchar_t)(*llp);
			llp++;
			data += 8;

			structure = (void *)llp;
			if (multiplier) {
				multiplier--;
			}
			if (multiplier == 0) {
				format++;
			}
		} else if (isdigit(fmt)) {
			multiplier = (multiplier * 10) + (fmt - '0');
			format++;
		} else {
			multiplier = 0;
			break;
		}
	}
}