[GitHub/mt8127/android_kernel_alcatel_ttab.git] / drivers / misc / cb710 / sgbuf2.c

/*
 *  cb710/sgbuf2.c
 *
 *  Copyright by Michał Mirosław, 2008-2009
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 */
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/cb710.h>

static bool sg_dwiter_next(struct sg_mapping_iter *miter)
{
	if (sg_miter_next(miter)) {
		miter->consumed = 0;
		return true;
	} else
		return false;
}

static bool sg_dwiter_is_at_end(struct sg_mapping_iter *miter)
{
	return miter->length == miter->consumed && !sg_dwiter_next(miter);
}

static uint32_t sg_dwiter_read_buffer(struct sg_mapping_iter *miter)
{
	size_t len, left = 4;
	uint32_t data;
	void *addr = &data;

	do {
		len = min(miter->length - miter->consumed, left);
		memcpy(addr, miter->addr + miter->consumed, len);
		miter->consumed += len;
		left -= len;
		if (!left)
			return data;
		addr += len;
	} while (sg_dwiter_next(miter));

	memset(addr, 0, left);
	return data;
}

static inline bool needs_unaligned_copy(const void *ptr)
{
#ifdef CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS
	return false;
#else
	return ((ptr - NULL) & 3) != 0;
#endif
}

static bool sg_dwiter_get_next_block(struct sg_mapping_iter *miter, uint32_t **ptr)
{
	size_t len;

	if (sg_dwiter_is_at_end(miter))
		return true;

	len = miter->length - miter->consumed;

	if (likely(len >= 4 && !needs_unaligned_copy(
			miter->addr + miter->consumed))) {
		*ptr = miter->addr + miter->consumed;
		miter->consumed += 4;
		return true;
	}

	return false;
}

/**
 * cb710_sg_dwiter_read_next_block() - get next 32-bit word from sg buffer
 * @miter: sg mapping iterator used for reading
 *
 * Description:
 *   Returns 32-bit word starting at byte pointed to by @miter@
 *   handling any alignment issues.  Bytes past the buffer's end
 *   are not accessed (read) but are returned as zeroes.  @miter@
 *   is advanced by 4 bytes or to the end of buffer whichever is
 *   closer.
 *
 * Context:
 *   Same requirements as in sg_miter_next().
 *
 * Returns:
 *   32-bit word just read.
 */
uint32_t cb710_sg_dwiter_read_next_block(struct sg_mapping_iter *miter)
{
	uint32_t *ptr = NULL;

	if (likely(sg_dwiter_get_next_block(miter, &ptr)))
		return ptr ? *ptr : 0;

	return sg_dwiter_read_buffer(miter);
}
EXPORT_SYMBOL_GPL(cb710_sg_dwiter_read_next_block);

static void sg_dwiter_write_slow(struct sg_mapping_iter *miter, uint32_t data)
{
	size_t len, left = 4;
	void *addr = &data;

	do {
		len = min(miter->length - miter->consumed, left);
		memcpy(miter->addr, addr, len);
		miter->consumed += len;
		left -= len;
		if (!left)
			return;
		addr += len;
	} while (sg_dwiter_next(miter));
}

/**
 * cb710_sg_dwiter_write_next_block() - write next 32-bit word to sg buffer
 * @miter: sg mapping iterator used for writing
 *
 * Description:
 *   Writes 32-bit word starting at byte pointed to by @miter@
 *   handling any alignment issues.  Bytes which would be written
 *   past the buffer's end are silently discarded. @miter@ is
 *   advanced by 4 bytes or to the end of buffer whichever is closer.
 *
 * Context:
 *   Same requirements as in sg_miter_next().
 */
void cb710_sg_dwiter_write_next_block(struct sg_mapping_iter *miter, uint32_t data)
{
	uint32_t *ptr = NULL;

	if (likely(sg_dwiter_get_next_block(miter, &ptr))) {
		if (ptr)
			*ptr = data;
		else
			return;
	} else
		sg_dwiter_write_slow(miter, data);
}
EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block);
Commit	Line	Data
5f5bac82 MM	1	/*
	2	* cb710/sgbuf2.c
	3	*
	4	* Copyright by Michał Mirosław, 2008-2009
	5	*
	6	* This program is free software; you can redistribute it and/or modify
	7	* it under the terms of the GNU General Public License version 2 as
	8	* published by the Free Software Foundation.
	9	*/
	10	#include <linux/kernel.h>
	11	#include <linux/module.h>
	12	#include <linux/cb710.h>
	13
	14	static bool sg_dwiter_next(struct sg_mapping_iter *miter)
	15	{
	16	if (sg_miter_next(miter)) {
	17	miter->consumed = 0;
	18	return true;
	19	} else
	20	return false;
	21	}
	22
	23	static bool sg_dwiter_is_at_end(struct sg_mapping_iter *miter)
	24	{
	25	return miter->length == miter->consumed && !sg_dwiter_next(miter);
	26	}
	27
	28	static uint32_t sg_dwiter_read_buffer(struct sg_mapping_iter *miter)
	29	{
	30	size_t len, left = 4;
	31	uint32_t data;
	32	void *addr = &data;
	33
	34	do {
	35	len = min(miter->length - miter->consumed, left);
	36	memcpy(addr, miter->addr + miter->consumed, len);
	37	miter->consumed += len;
	38	left -= len;
	39	if (!left)
	40	return data;
	41	addr += len;
	42	} while (sg_dwiter_next(miter));
	43
	44	memset(addr, 0, left);
	45	return data;
	46	}
	47
	48	static inline bool needs_unaligned_copy(const void *ptr)
	49	{
e9e8bcb8	50	#ifdef CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS
5f5bac82 MM	51	return false;
	52	#else
	53	return ((ptr - NULL) & 3) != 0;
	54	#endif
	55	}
	56
	57	static bool sg_dwiter_get_next_block(struct sg_mapping_iter miter, uint32_t *ptr)
	58	{
	59	size_t len;
	60
	61	if (sg_dwiter_is_at_end(miter))
	62	return true;
	63
	64	len = miter->length - miter->consumed;
	65
	66	if (likely(len >= 4 && !needs_unaligned_copy(
	67	miter->addr + miter->consumed))) {
	68	*ptr = miter->addr + miter->consumed;
	69	miter->consumed += 4;
	70	return true;
	71	}
	72
	73	return false;
	74	}
	75
	76	/**
	77	* cb710_sg_dwiter_read_next_block() - get next 32-bit word from sg buffer
	78	* @miter: sg mapping iterator used for reading
	79	*
	80	* Description:
	81	* Returns 32-bit word starting at byte pointed to by @miter@
	82	* handling any alignment issues. Bytes past the buffer's end
	83	* are not accessed (read) but are returned as zeroes. @miter@
	84	* is advanced by 4 bytes or to the end of buffer whichever is
	85	* closer.
	86	*
	87	* Context:
	88	* Same requirements as in sg_miter_next().
	89	*
	90	* Returns:
	91	* 32-bit word just read.
	92	*/
	93	uint32_t cb710_sg_dwiter_read_next_block(struct sg_mapping_iter *miter)
	94	{
	95	uint32_t *ptr = NULL;
	96
	97	if (likely(sg_dwiter_get_next_block(miter, &ptr)))
	98	return ptr ? *ptr : 0;
	99
	100	return sg_dwiter_read_buffer(miter);
	101	}
	102	EXPORT_SYMBOL_GPL(cb710_sg_dwiter_read_next_block);
	103
	104	static void sg_dwiter_write_slow(struct sg_mapping_iter *miter, uint32_t data)
	105	{
	106	size_t len, left = 4;
	107	void *addr = &data;
	108
	109	do {
	110	len = min(miter->length - miter->consumed, left);
	111	memcpy(miter->addr, addr, len);
	112	miter->consumed += len;
	113	left -= len;
	114	if (!left)
115	return;
116	addr += len;
5f5bac82 MM	117	} while (sg_dwiter_next(miter));
	118	}
	119
	120	/**
	121	* cb710_sg_dwiter_write_next_block() - write next 32-bit word to sg buffer
	122	* @miter: sg mapping iterator used for writing
	123	*
	124	* Description:
	125	* Writes 32-bit word starting at byte pointed to by @miter@
	126	* handling any alignment issues. Bytes which would be written
	127	* past the buffer's end are silently discarded. @miter@ is
	128	* advanced by 4 bytes or to the end of buffer whichever is closer.
	129	*
	130	* Context:
	131	* Same requirements as in sg_miter_next().
	132	*/
	133	void cb710_sg_dwiter_write_next_block(struct sg_mapping_iter *miter, uint32_t data)
	134	{
	135	uint32_t *ptr = NULL;
	136
	137	if (likely(sg_dwiter_get_next_block(miter, &ptr))) {
	138	if (ptr)
	139	*ptr = data;
	140	else
	141	return;
	142	} else
	143	sg_dwiter_write_slow(miter, data);
5f5bac82 MM	144	}
	145	EXPORT_SYMBOL_GPL(cb710_sg_dwiter_write_next_block);
	146