lwis_ioreg.c - kernel/google-modules/lwis - Git at Google

 /*
  * Google LWIS Register I/O Interface
  *
  * Copyright (c) 2018 Google, LLC
  *
  * 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.
  */

 #define pr_fmt(fmt) KBUILD_MODNAME "-ioreg: " fmt

 #include <linux/bitops.h>
 #include <linux/io.h>
 #include <linux/kernel.h>
 #include <linux/slab.h>

 #include "lwis_device.h"
 #include "lwis_ioreg.h"

 static int find_block_idx_by_name(struct lwis_ioreg_list *list, char *name)
 {
 	int i;
 	for (i = 0; i < list->count; ++i) {
 		if (!strcmp(list->block[i].name, name)) {
 			return i;
 		}
 	}
 	return -ENOENT;
 }

 static struct lwis_ioreg *get_block_by_idx(struct lwis_ioreg_device *ioreg_dev, int index)
 {
 	struct lwis_ioreg *block;
 	struct lwis_ioreg_list *list;

 	list = &ioreg_dev->reg_list;
 	if (index < 0 || index >= list->count) {
 		return ERR_PTR(-EINVAL);
 	}

 	block = &list->block[index];
 	if (!block->base) {
 		return ERR_PTR(-EINVAL);
 	}

 	return block;
 }

 static int validate_offset(struct lwis_ioreg_device *ioreg_dev, struct lwis_ioreg *block,
 			   uint64_t offset, size_t size_in_bytes, unsigned int alignment)
 {
 	uint64_t max_uint64 = 0xFFFFFFFFFFFFFFFFll;

 	if (offset % alignment) {
 		dev_err(ioreg_dev->base_dev.dev, "Accessing invalid address! Alignment error\n");
 		return -EFAULT;
 	}

 	if ((offset > max_uint64 - size_in_bytes) || (offset + size_in_bytes > block->size)) {
 		dev_err(ioreg_dev->base_dev.dev, "Accessing invalid address! Block size is %d.\n",
 			block->size);
 		dev_err(ioreg_dev->base_dev.dev,
 			"Offset %llu, size_in_bytes %zu, will be out of bound.\n", offset,
 			size_in_bytes);
 		return -EFAULT;
 	}
 	return 0;
 }

 static int validate_access_size(int access_size, int native_value_bitwidth)
 {
 	if (access_size != native_value_bitwidth) {
 		if (access_size != 8 && access_size != 16 && access_size != 32 &&
 		    access_size != 64) {
 			pr_err("Access size must be 8, 16, 32 or 64 - Actual %d\n", access_size);
 			return -EINVAL;
 		}
 		if (access_size > native_value_bitwidth) {
 			pr_err("Access size (%d) > bitwidth (%d) is not supported yet\n",
 			       access_size, native_value_bitwidth);
 			return -ENOSYS;
 		}
 	}
 	return 0;
 }

 int lwis_ioreg_list_alloc(struct lwis_ioreg_device *ioreg_dev, int num_blocks)
 {
 	struct lwis_ioreg_list *list;

 	if (!ioreg_dev) {
 		pr_err("LWIS IOREG device is NULL\n");
 		return -ENODEV;
 	}

 	/* No need to allocate if num_blocks is invalid */
 	if (num_blocks <= 0) {
 		return -EINVAL;
 	}

 	list = &ioreg_dev->reg_list;
 	list->block = kmalloc(num_blocks * sizeof(struct lwis_ioreg), GFP_KERNEL);
 	if (!list->block) {
 		return -ENOMEM;
 	}

 	list->count = num_blocks;

 	return 0;
 }

 void lwis_ioreg_list_free(struct lwis_ioreg_device *ioreg_dev)
 {
 	struct lwis_ioreg_list *list;

 	if (!ioreg_dev) {
 		pr_err("LWIS IOREG device is NULL\n");
 		return;
 	}

 	list = &ioreg_dev->reg_list;
 	if (list->block) {
 		kfree(list->block);
 		list->block = NULL;
 		list->count = 0;
 	}
 }

 int lwis_ioreg_get(struct lwis_ioreg_device *ioreg_dev, int index, char *name)
 {
 	struct resource *res;
 	struct lwis_ioreg *block;
 	struct lwis_ioreg_list *list;
 	struct platform_device *plat_dev;

 	if (!ioreg_dev) {
 		pr_err("LWIS IOREG device is NULL\n");
 		return -ENODEV;
 	}

 	plat_dev = ioreg_dev->base_dev.plat_dev;
 	list = &ioreg_dev->reg_list;
 	if (index < 0 || index >= list->count) {
 		return -EINVAL;
 	}

 	res = platform_get_resource(plat_dev, IORESOURCE_MEM, index);
 	if (!res) {
 		dev_err(ioreg_dev->base_dev.dev, "platform_get_resource error\n");
 		return -EINVAL;
 	}

 	block = &list->block[index];
 	block->name = name;
 	block->start = res->start;
 	block->size = resource_size(res);
 	block->base = devm_ioremap(&plat_dev->dev, res->start, resource_size(res));
 	if (!block->base) {
 		dev_err(ioreg_dev->base_dev.dev, "Cannot map I/O register space\n");
 		return -EINVAL;
 	}

 	return 0;
 }

 int lwis_ioreg_put_by_idx(struct lwis_ioreg_device *ioreg_dev, int index)
 {
 	struct lwis_ioreg_list *list;
 	struct device *dev;

 	if (!ioreg_dev) {
 		pr_err("LWIS IOREG device is NULL\n");
 		return -ENODEV;
 	};

 	dev = &ioreg_dev->base_dev.plat_dev->dev;
 	list = &ioreg_dev->reg_list;
 	if (index < 0 || index >= list->count) {
 		return -EINVAL;
 	}

 	if (!list->block[index].base) {
 		return -EINVAL;
 	}

 	devm_iounmap(dev, list->block[index].base);

 	return 0;
 }

 int lwis_ioreg_put_by_name(struct lwis_ioreg_device *ioreg_dev, char *name)
 {
 	int bidx;
 	struct lwis_ioreg_list *list;
 	struct device *dev;

 	if (!ioreg_dev) {
 		pr_err("LWIS IOREG device is NULL\n");
 		return -ENODEV;
 	};

 	dev = &ioreg_dev->base_dev.plat_dev->dev;
 	list = &ioreg_dev->reg_list;
 	bidx = find_block_idx_by_name(list, name);
 	if (bidx < 0) {
 		return bidx;
 	}
 	if (list->block[bidx].base == NULL) {
 		return -EINVAL;
 	}

 	devm_iounmap(dev, list->block[bidx].base);
 	return 0;
 }

 static int ioreg_read_batch_internal(void __iomem *base, uint64_t offset, int value_bits,
 				     size_t size_in_bytes, uint8_t *buf)
 {
 	int i;
 	uint8_t *addr = (uint8_t *)base + offset;

 	if (size_in_bytes & (value_bits / 8) - 1) {
 		pr_err("Read buf size (%zu) not divisible by %d (bitwidth = %d)\n", size_in_bytes,
 		       value_bits / 8, value_bits);
 		return -EINVAL;
 	}

 	switch (value_bits) {
 	case 8:
 		for (i = 0; i < size_in_bytes; ++i) {
 			*(buf + i) = readb_relaxed((void __iomem *)(addr + i));
 		}
 		break;
 	case 16:
 		for (i = 0; i < size_in_bytes; i += 2) {
 			*(uint16_t *)(buf + i) = readw_relaxed((void __iomem *)(addr + i));
 		}
 		break;
 	case 32:
 		for (i = 0; i < size_in_bytes; i += 4) {
 			*(uint32_t *)(buf + i) = readl_relaxed((void __iomem *)(addr + i));
 		}
 		break;
 	case 64:
 		for (i = 0; i < size_in_bytes; i += 8) {
 			*(uint64_t *)(buf + i) = readq_relaxed((void __iomem *)(addr + i));
 		}
 		break;
 	default:
 		return -EINVAL;
 	}

 	return 0;
 }

 static int ioreg_write_batch_internal(void __iomem *base, uint64_t offset, int value_bits,
 				      size_t size_in_bytes, uint8_t *buf, bool is_offset_fixed)
 {
 	int i;
 	uint8_t *addr = (uint8_t *)base + offset;

 	if (size_in_bytes & (value_bits / 8) - 1) {
 		pr_err("Write buf size (%zu) not divisible by %d (bitwidth = %d)\n", size_in_bytes,
 		       value_bits / 8, value_bits);
 		return -EINVAL;
 	}

 	switch (value_bits) {
 	case 8:
 		for (i = 0; i < size_in_bytes; ++i) {
 			writeb_relaxed(*(buf + i), is_offset_fixed ? (void __iomem *)(addr) :
 									   (void __iomem *)(addr + i));
 		}
 		break;
 	case 16:
 		for (i = 0; i < size_in_bytes; i += 2) {
 			writew_relaxed(*(uint16_t *)(buf + i), is_offset_fixed ?
 								       (void __iomem *)(addr) :
 									     (void __iomem *)(addr + i));
 		}
 		break;
 	case 32:
 		for (i = 0; i < size_in_bytes; i += 4) {
 			writel_relaxed(*(uint32_t *)(buf + i), is_offset_fixed ?
 								       (void __iomem *)(addr) :
 									     (void __iomem *)(addr + i));
 		}
 		break;
 	case 64:
 		for (i = 0; i < size_in_bytes; i += 8) {
 			writeq_relaxed(*(uint64_t *)(buf + i), is_offset_fixed ?
 								       (void __iomem *)(addr) :
 									     (void __iomem *)(addr + i));
 		}
 		break;
 	default:
 		return -EINVAL;
 	}

 	return 0;
 }

 static int ioreg_read_internal(void __iomem *base, uint64_t offset, int value_bits, uint64_t *value)
 {
 	void __iomem *addr = (void __iomem *)((uint8_t *)base + offset);
 	switch (value_bits) {
 	case 8:
 		*value = readb_relaxed(addr);
 		break;
 	case 16:
 		*value = readw_relaxed(addr);
 		break;
 	case 32:
 		*value = readl_relaxed(addr);
 		break;
 	case 64:
 		*value = readq_relaxed(addr);
 		break;
 	default:
 		return -EINVAL;
 	}

 	return 0;
 }

 static int ioreg_write_internal(void __iomem *base, uint64_t offset, int value_bits, uint64_t value)
 {
 	void __iomem *addr = (void __iomem *)((uint8_t *)base + offset);

 	switch (value_bits) {
 	case 8:
 		writeb_relaxed((uint8_t)value, addr);
 		break;
 	case 16:
 		writew_relaxed((uint16_t)value, addr);
 		break;
 	case 32:
 		writel_relaxed((uint32_t)value, addr);
 		break;
 	case 64:
 		writeq_relaxed(value, addr);
 		break;
 	default:
 		return -EINVAL;
 	}

 	return 0;
 }

 int lwis_ioreg_io_entry_rw(struct lwis_ioreg_device *ioreg_dev, struct lwis_io_entry *entry,
 			   int access_size)
 {
 	int ret = 0;
 	int index;
 	struct lwis_ioreg *block;
 	uint64_t reg_value;

 	if (!ioreg_dev) {
 		pr_err("LWIS IOREG device is NULL\n");
 		return -ENODEV;
 	};

 	if (!entry) {
 		dev_err(ioreg_dev->base_dev.dev, "IO entry is NULL.\n");
 		return -EINVAL;
 	}

 	/* Non-blocking because we already locked here */
 	if (entry->type == LWIS_IO_ENTRY_READ) {
 		ret = lwis_ioreg_read(ioreg_dev, entry->rw.bid, entry->rw.offset, &entry->rw.val,
 				      access_size);
 		if (ret) {
 			dev_err(ioreg_dev->base_dev.dev,
 				"ioreg read failed at: Bid: %d, Offset: 0x%llx\n", entry->rw.bid,
 				entry->rw.offset);
 		}
 	} else if (entry->type == LWIS_IO_ENTRY_READ_BATCH) {
 		index = entry->rw_batch.bid;
 		block = get_block_by_idx(ioreg_dev, index);
 		if (IS_ERR_OR_NULL(block)) {
 			return PTR_ERR(block);
 		}

 		ret = validate_offset(ioreg_dev, block, entry->rw_batch.offset,
 				      entry->rw_batch.size_in_bytes,
 				      ioreg_dev->base_dev.native_addr_bitwidth / 8);
 		if (ret) {
 			dev_err(ioreg_dev->base_dev.dev,
 				"ioreg validate_offset failed at: Offset: 0x%llx\n",
 				entry->rw_batch.offset);
 			return ret;
 		}

 		ret = ioreg_read_batch_internal(block->base, entry->rw_batch.offset,
 						ioreg_dev->base_dev.native_value_bitwidth,
 						entry->rw_batch.size_in_bytes, entry->rw_batch.buf);
 		if (ret) {
 			dev_err(ioreg_dev->base_dev.dev, "Invalid ioreg batch read at:\n");
 			dev_err(ioreg_dev->base_dev.dev, "Offset: 0x%llx, Base: %pK\n",
 				entry->rw_batch.offset, block->base);
 		}
 	} else if (entry->type == LWIS_IO_ENTRY_WRITE) {
 		ret = lwis_ioreg_write(ioreg_dev, entry->rw.bid, entry->rw.offset, entry->rw.val,
 				       access_size);
 		if (ret) {
 			dev_err(ioreg_dev->base_dev.dev,
 				"ioreg write failed at: Bid: %d, Offset: 0x%llx\n", entry->rw.bid,
 				entry->rw.offset);
 		}
 	} else if (entry->type == LWIS_IO_ENTRY_WRITE_BATCH) {
 		if (ioreg_dev->base_dev.is_read_only) {
 			dev_err(ioreg_dev->base_dev.dev, "Device is read only\n");
 			return -EPERM;
 		}

 		index = entry->rw_batch.bid;
 		block = get_block_by_idx(ioreg_dev, index);
 		if (IS_ERR_OR_NULL(block)) {
 			return PTR_ERR(block);
 		}

 		ret = validate_offset(ioreg_dev, block, entry->rw_batch.offset,
 				      entry->rw_batch.size_in_bytes,
 				      ioreg_dev->base_dev.native_addr_bitwidth / 8);
 		if (ret) {
 			dev_err(ioreg_dev->base_dev.dev,
 				"ioreg validate_offset failed at: Offset: 0x%llx\n",
 				entry->rw_batch.offset);
 			return ret;
 		}
 		ret = ioreg_write_batch_internal(block->base, entry->rw_batch.offset,
 						 ioreg_dev->base_dev.native_value_bitwidth,
 						 entry->rw_batch.size_in_bytes, entry->rw_batch.buf,
 						 entry->rw_batch.is_offset_fixed);
 		if (ret) {
 			dev_err(ioreg_dev->base_dev.dev, "Invalid ioreg batch write at:\n");
 			dev_err(ioreg_dev->base_dev.dev, "Offset: 0x%08llx, Base: %pK\n",
 				entry->rw_batch.offset, block->base);
 		}
 	} else if (entry->type == LWIS_IO_ENTRY_MODIFY) {
 		ret = lwis_ioreg_read(ioreg_dev, entry->mod.bid, entry->mod.offset, &reg_value,
 				      access_size);
 		if (ret) {
 			dev_err(ioreg_dev->base_dev.dev,
 				"ioreg modify read failed at: Bid: %d, Offset: 0x%llx\n",
 				entry->mod.bid, entry->mod.offset);
 			return ret;
 		}
 		reg_value &= ~entry->mod.val_mask;
 		reg_value |= entry->mod.val_mask & entry->mod.val;
 		ret = lwis_ioreg_write(ioreg_dev, entry->mod.bid, entry->mod.offset, reg_value,
 				       access_size);
 		if (ret) {
 			dev_err(ioreg_dev->base_dev.dev, "ioreg modify write failed at:");
 			dev_err(ioreg_dev->base_dev.dev, "Bid: %d, Offset: 0x%llx, Value: 0x%llx",
 				entry->mod.bid, entry->mod.offset, reg_value);
 		}
 	} else {
 		dev_err(ioreg_dev->base_dev.dev, "Invalid IO entry type: %d\n", entry->type);
 		return -EINVAL;
 	}

 	return ret;
 }

 int lwis_ioreg_read(struct lwis_ioreg_device *ioreg_dev, int index, uint64_t offset,
 		    uint64_t *value, int access_size)
 {
 	struct lwis_ioreg *block;
 	int ret;
 	uint64_t internal_offset = offset;
 	unsigned int native_value_bitwidth;
 	uint64_t offset_mask;

 	if (!ioreg_dev) {
 		pr_err("LWIS IOREG device is NULL\n");
 		return -ENODEV;
 	};

 	block = get_block_by_idx(ioreg_dev, index);
 	if (IS_ERR_OR_NULL(block)) {
 		return PTR_ERR(block);
 	}

 	native_value_bitwidth = ioreg_dev->base_dev.native_value_bitwidth;
 	ret = validate_access_size(access_size, native_value_bitwidth);
 	if (ret) {
 		dev_err(ioreg_dev->base_dev.dev, "Invalid access size\n");
 		return ret;
 	}
 	if (access_size != native_value_bitwidth) {
 		offset_mask = native_value_bitwidth / BITS_PER_BYTE - 1;
 		internal_offset = offset & ~offset_mask;
 	}

 	// Access_size is bitwidth
 	// and validate_offset expects size of bytes
 	ret = validate_offset(ioreg_dev, block, internal_offset, access_size / 8,
 			      ioreg_dev->base_dev.native_addr_bitwidth / 8);
 	if (ret) {
 		return ret;
 	}

 	ret = ioreg_read_internal(block->base, internal_offset, native_value_bitwidth, value);

 	if (access_size != native_value_bitwidth) {
 		*value >>= (offset - internal_offset) * BITS_PER_BYTE;
 		if (access_size < BITS_PER_TYPE(uint64_t)) {
 			*value &= ((1ULL << access_size) - 1);
 		}
 	}

 	return ret;
 }

 int lwis_ioreg_write(struct lwis_ioreg_device *ioreg_dev, int index, uint64_t offset,
 		     uint64_t value, int access_size)
 {
 	struct lwis_ioreg *block;
 	int ret;
 	uint64_t internal_offset = offset;
 	unsigned int native_value_bitwidth;
 	uint64_t read_value;
 	uint64_t offset_mask;
 	uint64_t value_mask;

 	if (!ioreg_dev) {
 		pr_err("LWIS IOREG device is NULL\n");
 		return -ENODEV;
 	};

 	if (ioreg_dev->base_dev.is_read_only) {
 		dev_err(ioreg_dev->base_dev.dev, "Device is read only\n");
 		return -EPERM;
 	}

 	block = get_block_by_idx(ioreg_dev, index);
 	if (IS_ERR_OR_NULL(block)) {
 		return PTR_ERR(block);
 	}

 	native_value_bitwidth = ioreg_dev->base_dev.native_value_bitwidth;
 	ret = validate_access_size(access_size, native_value_bitwidth);
 	if (ret) {
 		dev_err(ioreg_dev->base_dev.dev, "Invalid access size\n");
 		return ret;
 	}

 	if (access_size != native_value_bitwidth) {
 		offset_mask = native_value_bitwidth / BITS_PER_BYTE - 1;
 		internal_offset = offset & ~offset_mask;
 		value_mask = ((1 << access_size) - 1);
 		value_mask <<= (offset - internal_offset) * BITS_PER_BYTE;
 		/* We need to read-modify-write in this case */
 		ioreg_read_internal(block->base, internal_offset, native_value_bitwidth,
 				    &read_value);
 		value <<= (offset - internal_offset) * 8;
 		value = (value & value_mask) | (read_value & ~value_mask);
 	}

 	// Access_size is bitwidth
 	// and validate_offset expects size of bytes
 	ret = validate_offset(ioreg_dev, block, internal_offset, access_size / 8,
 			      ioreg_dev->base_dev.native_addr_bitwidth / 8);
 	if (ret) {
 		return ret;
 	}

 	return ioreg_write_internal(block->base, internal_offset, native_value_bitwidth, value);
 }

 int lwis_ioreg_set_io_barrier(struct lwis_ioreg_device *ioreg_dev, bool use_read_barrier,
 			      bool use_write_barrier)
 {
 	if (use_read_barrier) {
 		dma_rmb();
 	}
 	if (use_write_barrier) {
 		dma_wmb();
 	}
 	return 0;
 }
	/*
	* Google LWIS Register I/O Interface
	*
	* Copyright (c) 2018 Google, LLC
	*
	* 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.
	*/

	#define pr_fmt(fmt) KBUILD_MODNAME "-ioreg: " fmt

	#include <linux/bitops.h>
	#include <linux/io.h>
	#include <linux/kernel.h>
	#include <linux/slab.h>

	#include "lwis_device.h"
	#include "lwis_ioreg.h"

	static int find_block_idx_by_name(struct lwis_ioreg_list list, char name)
	{
	int i;
	for (i = 0; i < list->count; ++i) {
	if (!strcmp(list->block[i].name, name)) {
	return i;
	}
	}
	return -ENOENT;
	}

	static struct lwis_ioreg get_block_by_idx(struct lwis_ioreg_device ioreg_dev, int index)
	{
	struct lwis_ioreg *block;
	struct lwis_ioreg_list *list;

	list = &ioreg_dev->reg_list;
	if (index < 0 \|\| index >= list->count) {
	return ERR_PTR(-EINVAL);
	}

	block = &list->block[index];
	if (!block->base) {
	return ERR_PTR(-EINVAL);
	}

	return block;
	}

	static int validate_offset(struct lwis_ioreg_device ioreg_dev, struct lwis_ioreg block,
	uint64_t offset, size_t size_in_bytes, unsigned int alignment)
	{
	uint64_t max_uint64 = 0xFFFFFFFFFFFFFFFFll;

	if (offset % alignment) {
	dev_err(ioreg_dev->base_dev.dev, "Accessing invalid address! Alignment error\n");
	return -EFAULT;
	}

	if ((offset > max_uint64 - size_in_bytes) \|\| (offset + size_in_bytes > block->size)) {
	dev_err(ioreg_dev->base_dev.dev, "Accessing invalid address! Block size is %d.\n",
	block->size);
	dev_err(ioreg_dev->base_dev.dev,
	"Offset %llu, size_in_bytes %zu, will be out of bound.\n", offset,
	size_in_bytes);
	return -EFAULT;
	}
	return 0;
	}

	static int validate_access_size(int access_size, int native_value_bitwidth)
	{
	if (access_size != native_value_bitwidth) {
	if (access_size != 8 && access_size != 16 && access_size != 32 &&
	access_size != 64) {
	pr_err("Access size must be 8, 16, 32 or 64 - Actual %d\n", access_size);
	return -EINVAL;
	}
	if (access_size > native_value_bitwidth) {
	pr_err("Access size (%d) > bitwidth (%d) is not supported yet\n",
	access_size, native_value_bitwidth);
	return -ENOSYS;
	}
	}
	return 0;
	}

	int lwis_ioreg_list_alloc(struct lwis_ioreg_device *ioreg_dev, int num_blocks)
	{
	struct lwis_ioreg_list *list;

	if (!ioreg_dev) {
	pr_err("LWIS IOREG device is NULL\n");
	return -ENODEV;
	}

	/* No need to allocate if num_blocks is invalid */
	if (num_blocks <= 0) {
	return -EINVAL;
	}

	list = &ioreg_dev->reg_list;
	list->block = kmalloc(num_blocks * sizeof(struct lwis_ioreg), GFP_KERNEL);
	if (!list->block) {
	return -ENOMEM;
	}

	list->count = num_blocks;

	return 0;
	}

	void lwis_ioreg_list_free(struct lwis_ioreg_device *ioreg_dev)
	{
	struct lwis_ioreg_list *list;

	if (!ioreg_dev) {
	pr_err("LWIS IOREG device is NULL\n");
	return;
	}

	list = &ioreg_dev->reg_list;
	if (list->block) {
	kfree(list->block);
	list->block = NULL;
	list->count = 0;
	}
	}

	int lwis_ioreg_get(struct lwis_ioreg_device ioreg_dev, int index, char name)
	{
	struct resource *res;
	struct lwis_ioreg *block;
	struct lwis_ioreg_list *list;
	struct platform_device *plat_dev;

	if (!ioreg_dev) {
	pr_err("LWIS IOREG device is NULL\n");
	return -ENODEV;
	}

	plat_dev = ioreg_dev->base_dev.plat_dev;
	list = &ioreg_dev->reg_list;
	if (index < 0 \|\| index >= list->count) {
	return -EINVAL;
	}

	res = platform_get_resource(plat_dev, IORESOURCE_MEM, index);
	if (!res) {
	dev_err(ioreg_dev->base_dev.dev, "platform_get_resource error\n");
	return -EINVAL;
	}

	block = &list->block[index];
	block->name = name;
	block->start = res->start;
	block->size = resource_size(res);
	block->base = devm_ioremap(&plat_dev->dev, res->start, resource_size(res));
	if (!block->base) {
	dev_err(ioreg_dev->base_dev.dev, "Cannot map I/O register space\n");
	return -EINVAL;
	}

	return 0;
	}

	int lwis_ioreg_put_by_idx(struct lwis_ioreg_device *ioreg_dev, int index)
	{
	struct lwis_ioreg_list *list;
	struct device *dev;

	if (!ioreg_dev) {
	pr_err("LWIS IOREG device is NULL\n");
	return -ENODEV;
	};

	dev = &ioreg_dev->base_dev.plat_dev->dev;
	list = &ioreg_dev->reg_list;
	if (index < 0 \|\| index >= list->count) {
	return -EINVAL;
	}

	if (!list->block[index].base) {
	return -EINVAL;
	}

	devm_iounmap(dev, list->block[index].base);

	return 0;
	}

	int lwis_ioreg_put_by_name(struct lwis_ioreg_device ioreg_dev, char name)
	{
	int bidx;
	struct lwis_ioreg_list *list;
	struct device *dev;

	if (!ioreg_dev) {
	pr_err("LWIS IOREG device is NULL\n");
	return -ENODEV;
	};

	dev = &ioreg_dev->base_dev.plat_dev->dev;
	list = &ioreg_dev->reg_list;
	bidx = find_block_idx_by_name(list, name);
	if (bidx < 0) {
	return bidx;
	}
	if (list->block[bidx].base == NULL) {
	return -EINVAL;
	}

	devm_iounmap(dev, list->block[bidx].base);
	return 0;
	}

	static int ioreg_read_batch_internal(void __iomem *base, uint64_t offset, int value_bits,
	size_t size_in_bytes, uint8_t *buf)
	{
	int i;
	uint8_t addr = (uint8_t )base + offset;

	if (size_in_bytes & (value_bits / 8) - 1) {
	pr_err("Read buf size (%zu) not divisible by %d (bitwidth = %d)\n", size_in_bytes,
	value_bits / 8, value_bits);
	return -EINVAL;
	}

	switch (value_bits) {
	case 8:
	for (i = 0; i < size_in_bytes; ++i) {
	(buf + i) = readb_relaxed((void __iomem )(addr + i));
	}
	break;
	case 16:
	for (i = 0; i < size_in_bytes; i += 2) {
	(uint16_t )(buf + i) = readw_relaxed((void __iomem *)(addr + i));
	}
	break;
	case 32:
	for (i = 0; i < size_in_bytes; i += 4) {
	(uint32_t )(buf + i) = readl_relaxed((void __iomem *)(addr + i));
	}
	break;
	case 64:
	for (i = 0; i < size_in_bytes; i += 8) {
	(uint64_t )(buf + i) = readq_relaxed((void __iomem *)(addr + i));
	}
	break;
	default:
	return -EINVAL;
	}

	return 0;
	}

	static int ioreg_write_batch_internal(void __iomem *base, uint64_t offset, int value_bits,
	size_t size_in_bytes, uint8_t *buf, bool is_offset_fixed)
	{
	int i;
	uint8_t addr = (uint8_t )base + offset;

	if (size_in_bytes & (value_bits / 8) - 1) {
	pr_err("Write buf size (%zu) not divisible by %d (bitwidth = %d)\n", size_in_bytes,
	value_bits / 8, value_bits);
	return -EINVAL;
	}

	switch (value_bits) {
	case 8:
	for (i = 0; i < size_in_bytes; ++i) {
	writeb_relaxed((buf + i), is_offset_fixed ? (void __iomem )(addr) :
	(void __iomem *)(addr + i));
	}
	break;
	case 16:
	for (i = 0; i < size_in_bytes; i += 2) {
	writew_relaxed((uint16_t )(buf + i), is_offset_fixed ?
	(void __iomem *)(addr) :
	(void __iomem *)(addr + i));
	}
	break;
	case 32:
	for (i = 0; i < size_in_bytes; i += 4) {
	writel_relaxed((uint32_t )(buf + i), is_offset_fixed ?
	(void __iomem *)(addr) :
	(void __iomem *)(addr + i));
	}
	break;
	case 64:
	for (i = 0; i < size_in_bytes; i += 8) {
	writeq_relaxed((uint64_t )(buf + i), is_offset_fixed ?
	(void __iomem *)(addr) :
	(void __iomem *)(addr + i));
	}
	break;
	default:
	return -EINVAL;
	}

	return 0;
	}

	static int ioreg_read_internal(void __iomem base, uint64_t offset, int value_bits, uint64_t value)
	{
	void __iomem addr = (void __iomem )((uint8_t *)base + offset);
	switch (value_bits) {
	case 8:
	*value = readb_relaxed(addr);
	break;
	case 16:
	*value = readw_relaxed(addr);
	break;
	case 32:
	*value = readl_relaxed(addr);
	break;
	case 64:
	*value = readq_relaxed(addr);
	break;
	default:
	return -EINVAL;
	}

	return 0;
	}

	static int ioreg_write_internal(void __iomem *base, uint64_t offset, int value_bits, uint64_t value)
	{
	void __iomem addr = (void __iomem )((uint8_t *)base + offset);

	switch (value_bits) {
	case 8:
	writeb_relaxed((uint8_t)value, addr);
	break;
	case 16:
	writew_relaxed((uint16_t)value, addr);
	break;
	case 32:
	writel_relaxed((uint32_t)value, addr);
	break;
	case 64:
	writeq_relaxed(value, addr);
	break;
	default:
	return -EINVAL;
	}

	return 0;
	}

	int lwis_ioreg_io_entry_rw(struct lwis_ioreg_device ioreg_dev, struct lwis_io_entry entry,
	int access_size)
	{
	int ret = 0;
	int index;
	struct lwis_ioreg *block;
	uint64_t reg_value;

	if (!ioreg_dev) {
	pr_err("LWIS IOREG device is NULL\n");
	return -ENODEV;
	};

	if (!entry) {
	dev_err(ioreg_dev->base_dev.dev, "IO entry is NULL.\n");
	return -EINVAL;
	}

	/* Non-blocking because we already locked here */
	if (entry->type == LWIS_IO_ENTRY_READ) {
	ret = lwis_ioreg_read(ioreg_dev, entry->rw.bid, entry->rw.offset, &entry->rw.val,
	access_size);
	if (ret) {
	dev_err(ioreg_dev->base_dev.dev,
	"ioreg read failed at: Bid: %d, Offset: 0x%llx\n", entry->rw.bid,
	entry->rw.offset);
	}
	} else if (entry->type == LWIS_IO_ENTRY_READ_BATCH) {
	index = entry->rw_batch.bid;
	block = get_block_by_idx(ioreg_dev, index);
	if (IS_ERR_OR_NULL(block)) {
	return PTR_ERR(block);
	}

	ret = validate_offset(ioreg_dev, block, entry->rw_batch.offset,
	entry->rw_batch.size_in_bytes,
	ioreg_dev->base_dev.native_addr_bitwidth / 8);
	if (ret) {
	dev_err(ioreg_dev->base_dev.dev,
	"ioreg validate_offset failed at: Offset: 0x%llx\n",
	entry->rw_batch.offset);
	return ret;
	}

	ret = ioreg_read_batch_internal(block->base, entry->rw_batch.offset,
	ioreg_dev->base_dev.native_value_bitwidth,
	entry->rw_batch.size_in_bytes, entry->rw_batch.buf);
	if (ret) {
	dev_err(ioreg_dev->base_dev.dev, "Invalid ioreg batch read at:\n");
	dev_err(ioreg_dev->base_dev.dev, "Offset: 0x%llx, Base: %pK\n",
	entry->rw_batch.offset, block->base);
	}
	} else if (entry->type == LWIS_IO_ENTRY_WRITE) {
	ret = lwis_ioreg_write(ioreg_dev, entry->rw.bid, entry->rw.offset, entry->rw.val,
	access_size);
	if (ret) {
	dev_err(ioreg_dev->base_dev.dev,
	"ioreg write failed at: Bid: %d, Offset: 0x%llx\n", entry->rw.bid,
	entry->rw.offset);
	}
	} else if (entry->type == LWIS_IO_ENTRY_WRITE_BATCH) {
	if (ioreg_dev->base_dev.is_read_only) {
	dev_err(ioreg_dev->base_dev.dev, "Device is read only\n");
	return -EPERM;
	}

	index = entry->rw_batch.bid;
	block = get_block_by_idx(ioreg_dev, index);
	if (IS_ERR_OR_NULL(block)) {
	return PTR_ERR(block);
	}

	ret = validate_offset(ioreg_dev, block, entry->rw_batch.offset,
	entry->rw_batch.size_in_bytes,
	ioreg_dev->base_dev.native_addr_bitwidth / 8);
	if (ret) {
	dev_err(ioreg_dev->base_dev.dev,
	"ioreg validate_offset failed at: Offset: 0x%llx\n",
	entry->rw_batch.offset);
	return ret;
	}
	ret = ioreg_write_batch_internal(block->base, entry->rw_batch.offset,
	ioreg_dev->base_dev.native_value_bitwidth,
	entry->rw_batch.size_in_bytes, entry->rw_batch.buf,
	entry->rw_batch.is_offset_fixed);
	if (ret) {
	dev_err(ioreg_dev->base_dev.dev, "Invalid ioreg batch write at:\n");
	dev_err(ioreg_dev->base_dev.dev, "Offset: 0x%08llx, Base: %pK\n",
	entry->rw_batch.offset, block->base);
	}
	} else if (entry->type == LWIS_IO_ENTRY_MODIFY) {
	ret = lwis_ioreg_read(ioreg_dev, entry->mod.bid, entry->mod.offset, &reg_value,
	access_size);
	if (ret) {
	dev_err(ioreg_dev->base_dev.dev,
	"ioreg modify read failed at: Bid: %d, Offset: 0x%llx\n",
	entry->mod.bid, entry->mod.offset);
	return ret;
	}
	reg_value &= ~entry->mod.val_mask;
	reg_value \|= entry->mod.val_mask & entry->mod.val;
	ret = lwis_ioreg_write(ioreg_dev, entry->mod.bid, entry->mod.offset, reg_value,
	access_size);
	if (ret) {
	dev_err(ioreg_dev->base_dev.dev, "ioreg modify write failed at:");
	dev_err(ioreg_dev->base_dev.dev, "Bid: %d, Offset: 0x%llx, Value: 0x%llx",
	entry->mod.bid, entry->mod.offset, reg_value);
	}
	} else {
	dev_err(ioreg_dev->base_dev.dev, "Invalid IO entry type: %d\n", entry->type);
	return -EINVAL;
	}

	return ret;
	}

	int lwis_ioreg_read(struct lwis_ioreg_device *ioreg_dev, int index, uint64_t offset,
	uint64_t *value, int access_size)
	{
	struct lwis_ioreg *block;
	int ret;
	uint64_t internal_offset = offset;
	unsigned int native_value_bitwidth;
	uint64_t offset_mask;

	if (!ioreg_dev) {
	pr_err("LWIS IOREG device is NULL\n");
	return -ENODEV;
	};

	block = get_block_by_idx(ioreg_dev, index);
	if (IS_ERR_OR_NULL(block)) {
	return PTR_ERR(block);
	}

	native_value_bitwidth = ioreg_dev->base_dev.native_value_bitwidth;
	ret = validate_access_size(access_size, native_value_bitwidth);
	if (ret) {
	dev_err(ioreg_dev->base_dev.dev, "Invalid access size\n");
	return ret;
	}
	if (access_size != native_value_bitwidth) {
	offset_mask = native_value_bitwidth / BITS_PER_BYTE - 1;
	internal_offset = offset & ~offset_mask;
	}

	// Access_size is bitwidth
	// and validate_offset expects size of bytes
	ret = validate_offset(ioreg_dev, block, internal_offset, access_size / 8,
	ioreg_dev->base_dev.native_addr_bitwidth / 8);
	if (ret) {
	return ret;
	}

	ret = ioreg_read_internal(block->base, internal_offset, native_value_bitwidth, value);

	if (access_size != native_value_bitwidth) {
	value >>= (offset - internal_offset) BITS_PER_BYTE;
	if (access_size < BITS_PER_TYPE(uint64_t)) {
	*value &= ((1ULL << access_size) - 1);
	}
	}

	return ret;
	}

	int lwis_ioreg_write(struct lwis_ioreg_device *ioreg_dev, int index, uint64_t offset,
	uint64_t value, int access_size)
	{
	struct lwis_ioreg *block;
	int ret;
	uint64_t internal_offset = offset;
	unsigned int native_value_bitwidth;
	uint64_t read_value;
	uint64_t offset_mask;
	uint64_t value_mask;

	if (!ioreg_dev) {
	pr_err("LWIS IOREG device is NULL\n");
	return -ENODEV;
	};

	if (ioreg_dev->base_dev.is_read_only) {
	dev_err(ioreg_dev->base_dev.dev, "Device is read only\n");
	return -EPERM;
	}

	block = get_block_by_idx(ioreg_dev, index);
	if (IS_ERR_OR_NULL(block)) {
	return PTR_ERR(block);
	}

	native_value_bitwidth = ioreg_dev->base_dev.native_value_bitwidth;
	ret = validate_access_size(access_size, native_value_bitwidth);
	if (ret) {
	dev_err(ioreg_dev->base_dev.dev, "Invalid access size\n");
	return ret;
	}

	if (access_size != native_value_bitwidth) {
	offset_mask = native_value_bitwidth / BITS_PER_BYTE - 1;
	internal_offset = offset & ~offset_mask;
	value_mask = ((1 << access_size) - 1);
	value_mask <<= (offset - internal_offset) * BITS_PER_BYTE;
	/* We need to read-modify-write in this case */
	ioreg_read_internal(block->base, internal_offset, native_value_bitwidth,
	&read_value);
	value <<= (offset - internal_offset) * 8;
	value = (value & value_mask) \| (read_value & ~value_mask);
	}

	// Access_size is bitwidth
	// and validate_offset expects size of bytes
	ret = validate_offset(ioreg_dev, block, internal_offset, access_size / 8,
	ioreg_dev->base_dev.native_addr_bitwidth / 8);
	if (ret) {
	return ret;
	}

	return ioreg_write_internal(block->base, internal_offset, native_value_bitwidth, value);
	}

	int lwis_ioreg_set_io_barrier(struct lwis_ioreg_device *ioreg_dev, bool use_read_barrier,
	bool use_write_barrier)
	{
	if (use_read_barrier) {
	dma_rmb();
	}
	if (use_write_barrier) {
	dma_wmb();
	}
	return 0;
	}