LWIS: test infra refactor: add more functions for lwis-test device.

Bug: 259135155, 259134778
Test: atest LwisClientAndroidTest
Change-Id: Icfe67270ccc855e1bd8de63986a6ea276d63c2e6
Signed-off-by: linyuny <linyuny@google.com>

4 files changed, 150 insertions, 6 deletions

diff --git a/lwis_device_test.c b/lwis_device_test.c
index 75fd4dc..31b9ce5 100644
--- a/lwis_device_test.c
+++ b/lwis_device_test.c
@@ -17,6 +17,7 @@
 #include <linux/platform_device.h>
 
 #include "lwis_commands.h"
+#include "lwis_device.h"
 #include "lwis_init.h"
 #include "lwis_platform.h"
 
@@ -26,11 +27,16 @@
 
 #define LWIS_DRIVER_NAME "lwis-test"
 
+static int lwis_test_device_enable(struct lwis_device *lwis_dev);
+static int lwis_test_device_disable(struct lwis_device *lwis_dev);
+static int lwis_test_register_io(struct lwis_device *lwis_dev, struct lwis_io_entry *entry,
+				 int access_size);
+
 static struct lwis_device_subclass_operations test_vops = {
-	.register_io = NULL,
+	.register_io = lwis_test_register_io,
 	.register_io_barrier = NULL,
-	.device_enable = NULL,
-	.device_disable = NULL,
+	.device_enable = lwis_test_device_enable,
+	.device_disable = lwis_test_device_disable,
 	.event_enable = NULL,
 	.event_flags_updated = NULL,
 	.close = NULL,
@@ -43,6 +49,108 @@ static struct lwis_event_subscribe_operations test_subscribe_ops = {
 	.release = NULL,
 };
 
+static int lwis_test_device_enable(struct lwis_device *lwis_dev)
+{
+	return 0;
+}
+
+static int lwis_test_device_disable(struct lwis_device *lwis_dev)
+{
+	return 0;
+}
+
+static int lwis_test_register_io(struct lwis_device *lwis_dev, struct lwis_io_entry *entry,
+				 int access_size)
+{
+	struct lwis_test_device *test_dev =
+		container_of(lwis_dev, struct lwis_test_device, base_dev);
+	struct lwis_io_entry_rw_batch *rw_batch;
+	int i;
+	uint64_t reg_value;
+
+	if (!entry) {
+		dev_err(test_dev->base_dev.dev, "IO entry is NULL.\n");
+		return -EINVAL;
+	}
+
+	lwis_save_register_io_info(lwis_dev, entry, access_size);
+
+	if (entry->type == LWIS_IO_ENTRY_READ) {
+		if (entry->rw.offset >= SCRATCH_TEST_DEV_MEMORY_SIZE) {
+			dev_err(test_dev->base_dev.dev, "Offset (%llu) must be < %d\n",
+				entry->rw.offset, SCRATCH_TEST_DEV_MEMORY_SIZE);
+			return -EINVAL;
+		}
+		entry->rw.val = test_dev->scratch_mem[entry->rw.offset];
+	} else if (entry->type == LWIS_IO_ENTRY_READ_BATCH) {
+		rw_batch = &entry->rw_batch;
+		if (rw_batch->offset >= SCRATCH_TEST_DEV_MEMORY_SIZE ||
+		    SCRATCH_TEST_DEV_MEMORY_SIZE - rw_batch->offset < rw_batch->size_in_bytes) {
+			dev_err(test_dev->base_dev.dev,
+				"Read range[offset(%llu) + size_in_bytes(%zu)] exceeds scratch memory (%d)\n",
+				rw_batch->offset, rw_batch->size_in_bytes,
+				SCRATCH_TEST_DEV_MEMORY_SIZE);
+			return -EINVAL;
+		}
+		for (i = 0; i < rw_batch->size_in_bytes; ++i) {
+			rw_batch->buf[i] = test_dev->scratch_mem[rw_batch->offset + i];
+		}
+	} else if (entry->type == LWIS_IO_ENTRY_WRITE) {
+		if (entry->rw.offset >= SCRATCH_TEST_DEV_MEMORY_SIZE) {
+			dev_err(test_dev->base_dev.dev, "Offset (%llu) must be < %d\n",
+				entry->rw.offset, SCRATCH_TEST_DEV_MEMORY_SIZE);
+			return -EINVAL;
+		}
+		test_dev->scratch_mem[entry->rw.offset] = entry->rw.val;
+	} else if (entry->type == LWIS_IO_ENTRY_WRITE_BATCH) {
+		rw_batch = &entry->rw_batch;
+		if (rw_batch->offset >= SCRATCH_TEST_DEV_MEMORY_SIZE ||
+		    SCRATCH_TEST_DEV_MEMORY_SIZE - rw_batch->offset < rw_batch->size_in_bytes) {
+			dev_err(test_dev->base_dev.dev,
+				"Write range[offset(%llu) + size_in_bytes(%zu)] exceeds scratch memory (%d)\n",
+				rw_batch->offset, rw_batch->size_in_bytes,
+				SCRATCH_TEST_DEV_MEMORY_SIZE);
+			return -EINVAL;
+		}
+		for (i = 0; i < rw_batch->size_in_bytes; ++i) {
+			test_dev->scratch_mem[rw_batch->offset + i] = rw_batch->buf[i];
+		}
+	} else if (entry->type == LWIS_IO_ENTRY_MODIFY) {
+		if (entry->mod.offset >= SCRATCH_TEST_DEV_MEMORY_SIZE) {
+			dev_err(test_dev->base_dev.dev, "Offset (%llu) must be < %d\n",
+				entry->mod.offset, SCRATCH_TEST_DEV_MEMORY_SIZE);
+			return -EINVAL;
+		}
+		reg_value = test_dev->scratch_mem[entry->mod.offset];
+		reg_value &= ~entry->mod.val_mask;
+		reg_value |= entry->mod.val_mask & entry->mod.val;
+		test_dev->scratch_mem[entry->rw.offset] = reg_value;
+	} else {
+		dev_err(test_dev->base_dev.dev, "Invalid IO entry type: %d\n", entry->type);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int lwis_test_device_setup(struct lwis_test_device *test_dev)
+{
+	int ret = 0;
+
+#ifdef CONFIG_OF
+	/* Parse device tree for device configurations */
+	ret = lwis_test_device_parse_dt(test_dev);
+	if (ret) {
+		dev_err(test_dev->base_dev.dev, "Failed to parse device tree\n");
+	}
+#else
+	/* Non-device-tree init: Save for future implementation */
+	ret = -ENOSYS;
+#endif
+
+	return ret;
+}
+
 static int lwis_test_device_probe(struct platform_device *plat_dev)
 {
 	int ret = 0;
@@ -52,7 +160,7 @@ static int lwis_test_device_probe(struct platform_device *plat_dev)
 	/* Allocate test device specific data construct */
 	test_dev = devm_kzalloc(dev, sizeof(struct lwis_test_device), GFP_KERNEL);
 	if (!test_dev) {
-		dev_err(dev, "Failed to allocate test device structure\n");
+		dev_err(dev, "Failed to allocate TEST device structure\n");
 		return -ENOMEM;
 	}
 
@@ -63,9 +171,23 @@ static int lwis_test_device_probe(struct platform_device *plat_dev)
 	/* Call the base device probe function */
 	ret = lwis_base_probe(&test_dev->base_dev, plat_dev);
 	if (ret) {
-		dev_err(dev, "Error in lwis base probe, ret: %d\n", ret);
+		dev_err(dev, "TEST device: Error in lwis base probe: %d\n", ret);
+		goto error_probe;
+	}
+
+	/* Call TEST device specific setup function */
+	ret = lwis_test_device_setup(test_dev);
+	if (ret) {
+		dev_err(test_dev->base_dev.dev, "Error in TEST device initialization\n");
+		lwis_base_unprobe(&test_dev->base_dev);
+		goto error_probe;
 	}
 
+	dev_info(test_dev->base_dev.dev, "TEST Device Probe: Success\n");
+
+	return 0;
+
+error_probe:
 	return ret;
 }
 
@@ -111,6 +233,8 @@ int __init lwis_test_device_init(void)
 {
 	int ret = 0;
 
+	pr_info("TEST device initialization\n");
+
 	ret = platform_driver_register(&lwis_driver);
 	if (ret)
 		pr_err("platform_driver_register failed: %d\n", ret);
diff --git a/lwis_device_test.h b/lwis_device_test.h
index c3236e2..4012954 100644
--- a/lwis_device_test.h
+++ b/lwis_device_test.h
@@ -15,14 +15,21 @@
 #include "lwis_commands.h"
 #include "lwis_device.h"
 
+#define SCRATCH_TEST_DEV_MEMORY_SIZE 32
+
 /*
  *  struct lwis_test_device
  *  The device majorly control/handle requests from test clients.
  */
 struct lwis_test_device {
 	struct lwis_device base_dev;
+	/*
+	 * For testing purposes, scratch memory is used as register space in
+	 * test device.
+	 */
+	uint8_t scratch_mem[SCRATCH_TEST_DEV_MEMORY_SIZE];
 };
 
 int lwis_test_device_deinit(void);
 
-#endif /* LWIS_DEVICE_TEST_H_ */
\ No newline at end of file
+#endif /* LWIS_DEVICE_TEST_H_ */
diff --git a/lwis_dt.c b/lwis_dt.c
index da27d00..7dc09f3 100644
--- a/lwis_dt.c
+++ b/lwis_dt.c
@@ -1390,3 +1390,9 @@ int lwis_top_device_parse_dt(struct lwis_top_device *top_dev)
 	/* To be implemented */
 	return 0;
 }
+
+int lwis_test_device_parse_dt(struct lwis_test_device *test_dev)
+{
+	/* To be implemented */
+	return 0;
+}
diff --git a/lwis_dt.h b/lwis_dt.h
index 8d3195f..06ab764 100644
--- a/lwis_dt.h
+++ b/lwis_dt.h
@@ -16,6 +16,7 @@
 #include "lwis_device.h"
 #include "lwis_device_i2c.h"
 #include "lwis_device_ioreg.h"
+#include "lwis_device_test.h"
 #include "lwis_device_top.h"
 
 /*
@@ -42,4 +43,10 @@ int lwis_ioreg_device_parse_dt(struct lwis_ioreg_device *ioreg_dev);
  */
 int lwis_top_device_parse_dt(struct lwis_top_device *top_dev);
 
+/*
+ *  lwis_test_device_parse_dt: Parse device configurations specifically for
+ *  TEST devices.
+ */
+int lwis_test_device_parse_dt(struct lwis_test_device *test_dev);
+
 #endif /* LWIS_DT_H_ */