147 lines
3.4 KiB
C
147 lines
3.4 KiB
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 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);
|
|
|