in regmap/regmap.c [1659:1856]
static int _regmap_raw_write_impl(struct regmap *map, unsigned int reg,
const void *val, size_t val_len, bool noinc)
{
struct regmap_range_node *range;
unsigned long flags;
void *work_val = map->work_buf + map->format.reg_bytes +
map->format.pad_bytes;
void *buf;
int ret = -ENOTSUPP;
size_t len;
int i;
WARN_ON(!map->bus);
/* Check for unwritable or noinc registers in range
* before we start
*/
if (!regmap_writeable_noinc(map, reg)) {
for (i = 0; i < val_len / map->format.val_bytes; i++) {
unsigned int element =
reg + regmap_get_offset(map, i);
if (!regmap_writeable(map, element) ||
regmap_writeable_noinc(map, element))
return -EINVAL;
}
}
if (!map->cache_bypass && map->format.parse_val) {
unsigned int ival;
int val_bytes = map->format.val_bytes;
for (i = 0; i < val_len / val_bytes; i++) {
ival = map->format.parse_val(val + (i * val_bytes));
ret = regcache_write(map,
reg + regmap_get_offset(map, i),
ival);
if (ret) {
dev_err(map->dev,
"Error in caching of register: %x ret: %d\n",
reg + regmap_get_offset(map, i), ret);
return ret;
}
}
if (map->cache_only) {
map->cache_dirty = true;
return 0;
}
}
range = _regmap_range_lookup(map, reg);
if (range) {
int val_num = val_len / map->format.val_bytes;
int win_offset = (reg - range->range_min) % range->window_len;
int win_residue = range->window_len - win_offset;
/* If the write goes beyond the end of the window split it */
while (val_num > win_residue) {
dev_dbg(map->dev, "Writing window %d/%zu\n",
win_residue, val_len / map->format.val_bytes);
ret = _regmap_raw_write_impl(map, reg, val,
win_residue *
map->format.val_bytes, noinc);
if (ret != 0)
return ret;
reg += win_residue;
val_num -= win_residue;
val += win_residue * map->format.val_bytes;
val_len -= win_residue * map->format.val_bytes;
win_offset = (reg - range->range_min) %
range->window_len;
win_residue = range->window_len - win_offset;
}
ret = _regmap_select_page(map, ®, range, noinc ? 1 : val_num);
if (ret != 0)
return ret;
}
map->format.format_reg(map->work_buf, reg, map->reg_shift);
regmap_set_work_buf_flag_mask(map, map->format.reg_bytes,
map->write_flag_mask);
/*
* Essentially all I/O mechanisms will be faster with a single
* buffer to write. Since register syncs often generate raw
* writes of single registers optimise that case.
*/
if (val != work_val && val_len == map->format.val_bytes) {
memcpy(work_val, val, map->format.val_bytes);
val = work_val;
}
if (map->async && map->bus->async_write) {
struct regmap_async *async;
trace_regmap_async_write_start(map, reg, val_len);
spin_lock_irqsave(&map->async_lock, flags);
async = list_first_entry_or_null(&map->async_free,
struct regmap_async,
list);
if (async)
list_del(&async->list);
spin_unlock_irqrestore(&map->async_lock, flags);
if (!async) {
async = map->bus->async_alloc();
if (!async)
return -ENOMEM;
async->work_buf = kzalloc(map->format.buf_size,
GFP_KERNEL | GFP_DMA);
if (!async->work_buf) {
kfree(async);
return -ENOMEM;
}
}
async->map = map;
/* If the caller supplied the value we can use it safely. */
memcpy(async->work_buf, map->work_buf, map->format.pad_bytes +
map->format.reg_bytes + map->format.val_bytes);
spin_lock_irqsave(&map->async_lock, flags);
list_add_tail(&async->list, &map->async_list);
spin_unlock_irqrestore(&map->async_lock, flags);
if (val != work_val)
ret = map->bus->async_write(map->bus_context,
async->work_buf,
map->format.reg_bytes +
map->format.pad_bytes,
val, val_len, async);
else
ret = map->bus->async_write(map->bus_context,
async->work_buf,
map->format.reg_bytes +
map->format.pad_bytes +
val_len, NULL, 0, async);
if (ret != 0) {
dev_err(map->dev, "Failed to schedule write: %d\n",
ret);
spin_lock_irqsave(&map->async_lock, flags);
list_move(&async->list, &map->async_free);
spin_unlock_irqrestore(&map->async_lock, flags);
}
return ret;
}
trace_regmap_hw_write_start(map, reg, val_len / map->format.val_bytes);
/* If we're doing a single register write we can probably just
* send the work_buf directly, otherwise try to do a gather
* write.
*/
if (val == work_val)
ret = map->bus->write(map->bus_context, map->work_buf,
map->format.reg_bytes +
map->format.pad_bytes +
val_len);
else if (map->bus->gather_write)
ret = map->bus->gather_write(map->bus_context, map->work_buf,
map->format.reg_bytes +
map->format.pad_bytes,
val, val_len);
else
ret = -ENOTSUPP;
/* If that didn't work fall back on linearising by hand. */
if (ret == -ENOTSUPP) {
len = map->format.reg_bytes + map->format.pad_bytes + val_len;
buf = kzalloc(len, GFP_KERNEL);
if (!buf)
return -ENOMEM;
memcpy(buf, map->work_buf, map->format.reg_bytes);
memcpy(buf + map->format.reg_bytes + map->format.pad_bytes,
val, val_len);
ret = map->bus->write(map->bus_context, buf, len);
kfree(buf);
} else if (ret != 0 && !map->cache_bypass && map->format.parse_val) {
/* regcache_drop_region() takes lock that we already have,
* thus call map->cache_ops->drop() directly
*/
if (map->cache_ops && map->cache_ops->drop)
map->cache_ops->drop(map, reg, reg + 1);
}
trace_regmap_hw_write_done(map, reg, val_len / map->format.val_bytes);
return ret;
}