From a84c53fce4ccd67c147dcbb2dcf4fdeceab05981 Mon Sep 17 00:00:00 2001
From: "SkyLake.Huang" <skylake.huang@mediatek.com>
Date: Thu, 23 Jun 2022 18:35:52 +0800
Subject: [PATCH] drivers: spi: Add support for dynamic calibration

Signed-off-by: SkyLake.Huang <skylake.huang@mediatek.com>
---
 drivers/spi/spi.c       | 137 ++++++++++++++++++++++++++++++++++++++++
 include/linux/spi/spi.h |  42 ++++++++++++
 2 files changed, 179 insertions(+)

--- a/drivers/spi/spi.c
+++ b/drivers/spi/spi.c
@@ -1109,6 +1109,74 @@ static int spi_transfer_wait(struct spi_
 	return 0;
 }
 
+int spi_do_calibration(struct spi_controller *ctlr, struct spi_device *spi,
+	int (*cal_read)(void *priv, u32 *addr, int addrlen, u8 *buf, int readlen), void *drv_priv)
+{
+	int datalen, addrlen;
+	u8 *buf;
+	int ret;
+	int i;
+	struct list_head *pos, *tmp;
+	struct spi_cal_target *target;
+
+	/* Calculate calibration result */
+	int hit_val, total_hit, origin;
+	bool hit;
+
+	/* Make sure we can start calibration */
+	if(!ctlr->cal_target || !ctlr->cal_rule || !ctlr->append_caldata)
+		return -EINVAL;
+	datalen = ctlr->cal_rule->datalen;
+	addrlen = ctlr->cal_rule->addrlen;
+
+	buf = kzalloc(datalen * sizeof(u8), GFP_KERNEL);
+	if(!buf)
+		return -ENOMEM;
+
+	ret = ctlr->append_caldata(ctlr);
+	if (ret)
+		goto cal_end;
+
+	list_for_each_safe(pos, tmp, ctlr->cal_target) {
+		target = list_entry(pos, struct spi_cal_target, list);
+
+		hit = false;
+		hit_val = 0;
+		total_hit = 0;
+		origin = *target->cal_item;
+
+		for(i=target->cal_min; i<=target->cal_max; i+=target->step) {
+			*target->cal_item = i;
+			ret = (*cal_read)(drv_priv, ctlr->cal_rule->addr, addrlen, buf, datalen);
+			if(ret)
+				break;
+			dev_dbg(&spi->dev, "controller cal item value: 0x%x\n", i);
+			if(memcmp(ctlr->cal_rule->match_data, buf, datalen * sizeof(u8)) == 0) {
+				hit = true;
+				hit_val += i;
+				total_hit++;
+				dev_dbg(&spi->dev, "golden data matches data read!\n");
+			}
+		}
+		if(hit) {
+			*target->cal_item = DIV_ROUND_CLOSEST(hit_val, total_hit);
+			dev_info(&spi->dev, "calibration result: 0x%x", *target->cal_item);
+		} else {
+			*target->cal_item = origin;
+			dev_warn(&spi->dev, "calibration failed, fallback to default: 0x%x", origin);
+			ret = -EIO;
+		}
+
+		list_del(pos);
+		kfree(target);
+	}
+
+cal_end:
+	kfree(buf);
+	return ret? ret: 0;
+}
+EXPORT_SYMBOL_GPL(spi_do_calibration);
+
 static void _spi_transfer_delay_ns(u32 ns)
 {
 	if (!ns)
@@ -1720,6 +1788,75 @@ void spi_flush_queue(struct spi_controll
 /*-------------------------------------------------------------------------*/
 
 #if defined(CONFIG_OF)
+static inline void alloc_cal_data(struct list_head **cal_target,
+	struct spi_cal_rule **cal_rule, bool enable)
+{
+	if(enable) {
+		*cal_target = kmalloc(sizeof(struct list_head), GFP_KERNEL);
+		INIT_LIST_HEAD(*cal_target);
+		*cal_rule = kmalloc(sizeof(struct spi_cal_rule), GFP_KERNEL);
+	} else {
+		kfree(*cal_target);
+		kfree(*cal_rule);
+	}
+}
+
+static int of_spi_parse_cal_dt(struct spi_controller *ctlr, struct spi_device *spi,
+			   struct device_node *nc)
+{
+	u32 value;
+	int rc;
+	const char *cal_mode;
+
+	rc = of_property_read_bool(nc, "spi-cal-enable");
+	if (rc)
+		alloc_cal_data(&ctlr->cal_target, &ctlr->cal_rule, true);
+	else
+		return 0;
+
+	rc = of_property_read_string(nc, "spi-cal-mode", &cal_mode);
+	if(!rc) {
+		if(strcmp("read-data", cal_mode) == 0){
+			ctlr->cal_rule->mode = SPI_CAL_READ_DATA;
+		} else if(strcmp("read-pp", cal_mode) == 0) {
+			ctlr->cal_rule->mode = SPI_CAL_READ_PP;
+			return 0;
+		} else if(strcmp("read-sfdp", cal_mode) == 0){
+			ctlr->cal_rule->mode = SPI_CAL_READ_SFDP;
+			return 0;
+		}
+	} else
+		goto err;
+
+	ctlr->cal_rule->datalen = 0;
+	rc = of_property_read_u32(nc, "spi-cal-datalen", &value);
+	if(!rc && value > 0) {
+		ctlr->cal_rule->datalen = value;
+
+		ctlr->cal_rule->match_data = kzalloc(value * sizeof(u8), GFP_KERNEL);
+		rc = of_property_read_u8_array(nc, "spi-cal-data",
+				ctlr->cal_rule->match_data, value);
+		if(rc)
+			kfree(ctlr->cal_rule->match_data);
+	}
+
+	rc = of_property_read_u32(nc, "spi-cal-addrlen", &value);
+	if(!rc && value > 0) {
+		ctlr->cal_rule->addrlen = value;
+
+		ctlr->cal_rule->addr = kzalloc(value * sizeof(u32), GFP_KERNEL);
+		rc = of_property_read_u32_array(nc, "spi-cal-addr",
+				ctlr->cal_rule->addr, value);
+		if(rc)
+			kfree(ctlr->cal_rule->addr);
+	}
+	return 0;
+
+err:
+	alloc_cal_data(&ctlr->cal_target, &ctlr->cal_rule, false);
+	return 0;
+}
+
 static int of_spi_parse_dt(struct spi_controller *ctlr, struct spi_device *spi,
 			   struct device_node *nc)
 {
@@ -1841,6 +1978,10 @@ of_register_spi_device(struct spi_contro
 	if (rc)
 		goto err_out;
 
+	rc = of_spi_parse_cal_dt(ctlr, spi, nc);
+	if (rc)
+		goto err_out;
+
 	/* Store a pointer to the node in the device structure */
 	of_node_get(nc);
 	spi->dev.of_node = nc;
--- a/include/linux/spi/spi.h
+++ b/include/linux/spi/spi.h
@@ -264,6 +264,40 @@ struct spi_driver {
 	struct device_driver	driver;
 };
 
+enum {
+	SPI_CAL_READ_DATA = 0,
+	SPI_CAL_READ_PP = 1, /* only for SPI-NAND */
+	SPI_CAL_READ_SFDP = 2, /* only for SPI-NOR */
+};
+
+struct nand_addr {
+	unsigned int lun;
+	unsigned int plane;
+	unsigned int eraseblock;
+	unsigned int page;
+	unsigned int dataoffs;
+};
+
+/**
+ * Read calibration rule from device dts node.
+ * Once calibration result matches the rule, we regard is as success.
+ */
+struct spi_cal_rule {
+	int datalen;
+	u8 *match_data;
+	int addrlen;
+	u32 *addr;
+	int mode;
+};
+
+struct spi_cal_target {
+	u32 *cal_item;
+	int cal_min; /* min of cal_item */
+	int cal_max; /* max of cal_item */
+	int step; /* Increase/decrease cal_item */
+	struct list_head list;
+};
+
 static inline struct spi_driver *to_spi_driver(struct device_driver *drv)
 {
 	return drv ? container_of(drv, struct spi_driver, driver) : NULL;
@@ -606,6 +640,11 @@ struct spi_controller {
 	void			*dummy_rx;
 	void			*dummy_tx;
 
+	/* For calibration */
+	int (*append_caldata)(struct spi_controller *ctlr);
+	struct list_head *cal_target;
+	struct spi_cal_rule *cal_rule;
+
 	int (*fw_translate_cs)(struct spi_controller *ctlr, unsigned cs);
 };
 
@@ -1369,6 +1408,9 @@ spi_register_board_info(struct spi_board
 	{ return 0; }
 #endif
 
+extern int spi_do_calibration(struct spi_controller *ctlr,
+	struct spi_device *spi, int (*cal_read)(void *, u32 *, int, u8 *, int), void *drv_priv);
+
 /* If you're hotplugging an adapter with devices (parport, usb, etc)
  * use spi_new_device() to describe each device.  You can also call
  * spi_unregister_device() to start making that device vanish, but