Jetpack/kernel/kernel-4.9/drivers/extcon/extcon-cable-xlate.c

/*
 * extcon-cable-xlate: Cable translator based on different cable states.
 *
 * Copyright (c) 2014-2018, NVIDIA CORPORATION.  All rights reserved.
 *
 * Author: Laxman Dewangan <ldewangan@nvidia.com>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */

#include <linux/delay.h>
#include <linux/err.h>
#include <linux/module.h>
#include <linux/of.h>
#include <linux/of_device.h>
#include <linux/platform_device.h>
#include <linux/slab.h>
#include <linux/extcon.h>
#include <linux/spinlock.h>
#include <linux/device.h>

#define DEFAULT_CABLE_WAITTIME_MS		500
#define EXTCON_XLATE_WAKEUP_TIME		1000

struct ecx_io_cable_states {
	int in_state;
	int in_mask;
	int out_states;
};

struct ecx_io_cable_new_states {
	int last_cable_in_state;
	int current_in_cable_state;
	int in_mask;
	int new_cable_out_state;
	int reschedule_wq;
};

struct ecx_platform_data {
	const char *name;
	struct ecx_io_cable_states *io_states;
	struct ecx_io_cable_new_states *io_new_states;
	int n_io_states;
	int n_io_new_states;
	const char **in_cable_names;
	int n_in_cable;
	int *out_cable_names;
	int n_out_cable;
	int cable_insert_delay;
	int cable_detect_suspend_delay;
};

struct extcon_cable_xlate;

struct ecx_in_cables {
	struct extcon_cable_xlate *ecx;
	struct extcon_dev *ec_dev;
	struct notifier_block nb;
	struct extcon_specific_cable_nb ec_cable_nb;
	int cable;
};

struct extcon_cable_xlate {
	struct device *dev;
	struct ecx_platform_data *pdata;
	struct extcon_dev *edev;
	struct ecx_in_cables *in_cables;
	struct delayed_work work;
	struct timer_list timer;
	int debounce_jiffies;
	int timer_to_work_jiffies;
	spinlock_t lock;
	struct mutex cable_lock;
	struct wakeup_source wake_lock;
	bool extcon_init_done;
	int last_cable_in_state;
	int last_cable_out_state;
};

static int ecx_extcon_notifier(struct notifier_block *self,
			       unsigned long event, void *ptr);
static int ecx_init_input_cables(struct extcon_cable_xlate *ecx)
{
	struct device_node *np = ecx->dev->of_node;
	struct of_phandle_args npspec;
	struct ecx_in_cables *in_cables;
	int cindex;
	int i;
	int ret;

	for (i = 0; i < ecx->pdata->n_in_cable; i++) {
		in_cables = &ecx->in_cables[i];
		in_cables->ecx = ecx;
		in_cables->nb.notifier_call = ecx_extcon_notifier;
		in_cables->ec_dev = extcon_get_extcon_dev_by_cable(ecx->dev,
						ecx->pdata->in_cable_names[i]);
		if (IS_ERR(in_cables->ec_dev)) {
			ret = PTR_ERR(in_cables->ec_dev);
			if (ret != -EPROBE_DEFER)
				dev_err(ecx->dev,
					"extcon get failed for %s: %d\n",
					ecx->pdata->in_cable_names[i], ret);
			return ret;
		};

		ret = of_parse_phandle_with_args(np, "extcon-cables",
						"#extcon-cells", i, &npspec);
		if (ret < 0)
			return ret;
		cindex = npspec.args_count ? npspec.args[0] : 0;
		in_cables->cable = in_cables->ec_dev->supported_cable[cindex];
	}

	for (i = 0; i < ecx->pdata->n_in_cable; i++) {
		in_cables = &ecx->in_cables[i];
		ret = extcon_register_notifier(in_cables->ec_dev,
					       ecx->pdata->out_cable_names[i],
					       &in_cables->nb);
		if (ret < 0) {
			dev_err(ecx->dev, "Cable %s registration failed: %d\n",
				ecx->pdata->in_cable_names[i], ret);
			return ret;
		}
	}
	return 0;
}

static int ecx_attach_cable(struct extcon_cable_xlate *ecx)
{
	struct ecx_in_cables *in_cables;
	int mask_state = 0;
	int all_states = 0;
	int new_state = -1;
	int i;
	int ret;
	int reschedule_wq = 0;

	mutex_lock(&ecx->cable_lock);
	for (i = 0; i < ecx->pdata->n_in_cable; i++) {
		in_cables = &ecx->in_cables[i];

		ret = extcon_get_cable_state_(in_cables->ec_dev,
					      in_cables->cable);
		if (ret >= 1)
			all_states |= BIT(i);
	}

	if (ecx->pdata->io_new_states) {
		for (i = 0; i < ecx->pdata->n_io_new_states; ++i) {
			mask_state = all_states & ecx->pdata->io_new_states[i].in_mask;
			if (mask_state == ecx->pdata->io_new_states[i].current_in_cable_state) {
				if ((ecx->last_cable_in_state == mask_state) &&	mask_state) {
					mutex_unlock(&ecx->cable_lock);
					return 0;
				}

				if ((ecx->last_cable_in_state ==
						ecx->pdata->io_new_states[i].last_cable_in_state) ||
						(mask_state == 0)) {
					ecx->last_cable_in_state = mask_state;
					new_state = ecx->pdata->io_new_states[i].new_cable_out_state;
					reschedule_wq = ecx->pdata->io_new_states[i].reschedule_wq;
					break;
				}
			}
		}
	} else {
		for (i = 0; i < ecx->pdata->n_io_states; ++i) {
			mask_state = all_states & ecx->pdata->io_states[i].in_mask;
			if (mask_state == ecx->pdata->io_states[i].in_state) {
				new_state = ecx->pdata->io_states[i].out_states;
				break;
			}
		}
	}

	if (new_state < 0) {
		dev_err(ecx->dev, "Cable state 0x%04x is not defined\n",
			all_states);
		dev_err(ecx->dev,
			"Last cable in state: 0x%04x, mask state: 0x%04x\n",
			ecx->last_cable_in_state, mask_state);
		mutex_unlock(&ecx->cable_lock);
		return -EINVAL;
	}
	if (ecx->last_cable_out_state != new_state) {
		for (i = 0; i < ecx->pdata->n_out_cable; i++) {
			extcon_set_state_sync(ecx->edev,
					      ecx->pdata->out_cable_names[i],
					      !!(new_state & BIT(i)));
		}
		dev_info(ecx->dev, "New cable state 0x%04x\n", new_state);
		if (new_state) {
			i = ffs(new_state) - 1;
			dev_info(ecx->dev, "Cable%d %s is attach\n",
				 i, ecx->pdata->in_cable_names[i]);
		} else {
			ecx->last_cable_in_state = 0;
			dev_info(ecx->dev, "No cable attach\n");
		}
	}
	ecx->last_cable_out_state = new_state;
	if (reschedule_wq)
		schedule_delayed_work(&ecx->work, msecs_to_jiffies(1000));
	mutex_unlock(&ecx->cable_lock);
	return 0;
}

static void ecx_cable_state_update_work(struct work_struct *work)
{
	struct extcon_cable_xlate *ecx = container_of(to_delayed_work(work),
					struct extcon_cable_xlate, work);
	int ret;

	if (!ecx->extcon_init_done) {
		ret = ecx_init_input_cables(ecx);
		if (ret < 0) {
			if (ret == -EPROBE_DEFER)
				schedule_delayed_work(&ecx->work,
						      msecs_to_jiffies(1000));
			return;
		}
		dev_info(ecx->dev, "Extcon Init success\n");
		ecx->extcon_init_done = true;
		schedule_delayed_work(&ecx->work, msecs_to_jiffies(1000));
		return;
	}
	ecx_attach_cable(ecx);
}

static void ecx_extcon_notifier_timer(unsigned long _data)
{
	struct extcon_cable_xlate *ecx = (struct extcon_cable_xlate *)_data;

	schedule_delayed_work(&ecx->work, ecx->timer_to_work_jiffies);
}

static int ecx_extcon_notifier(struct notifier_block *self,
			       unsigned long event, void *ptr)
{
	struct ecx_in_cables *cable = container_of(self,
					struct ecx_in_cables, nb);
	struct extcon_cable_xlate *ecx = cable->ecx;
	unsigned long flags;

	/*Hold wakelock to complete cable detection */
	if (!(ecx->wake_lock.active))
		__pm_wakeup_event(&ecx->wake_lock,
				  ecx->pdata->cable_detect_suspend_delay);

	spin_lock_irqsave(&ecx->lock, flags);
	mod_timer(&ecx->timer, jiffies + ecx->debounce_jiffies);
	spin_unlock_irqrestore(&ecx->lock, flags);
	return 0;
}

static struct ecx_platform_data *ecx_get_pdata_from_dt(
	struct platform_device *pdev)
{
	struct ecx_platform_data *pdata;
	struct device_node *np = pdev->dev.of_node;
	u32 pval;
	int ret;
	const char *names;
	struct property *prop;
	int count;

	pdata = devm_kzalloc(&pdev->dev, sizeof(*pdata), GFP_KERNEL);
	if (!pdata)
		return ERR_PTR(-ENOMEM);

	ret = of_property_read_string(np, "extcon-name", &pdata->name);
	if (ret < 0)
		pdata->name = np->name;

	ret = of_property_read_u32(np, "cable-insert-delay", &pval);
	if (!ret)
		pdata->cable_insert_delay = pval;
	else
		pdata->cable_insert_delay = DEFAULT_CABLE_WAITTIME_MS;

	ret = of_property_read_u32(np, "cable-detect-suspend-delay", &pval);
	if (!ret)
		pdata->cable_detect_suspend_delay = pval;
	else
		pdata->cable_detect_suspend_delay = EXTCON_XLATE_WAKEUP_TIME;

	pdata->n_out_cable = of_property_count_u32_elems(np,
					"output-cable-names");
	if (pdata->n_out_cable <= 0) {
		dev_err(&pdev->dev, "Not found output cable names\n");
		return ERR_PTR(-EINVAL);
	}
	pdata->out_cable_names = devm_kzalloc(&pdev->dev,
				(pdata->n_out_cable) *
				sizeof(*pdata->out_cable_names), GFP_KERNEL);
	if (!pdata->out_cable_names)
		return ERR_PTR(-ENOMEM);

	ret = of_property_read_u32_array(np, "output-cable-names",
					 pdata->out_cable_names,
					 pdata->n_out_cable);
	if (ret)
		return ERR_PTR(-EINVAL);

	pdata->n_in_cable = of_property_count_strings(np, "extcon-cable-names");
	if (pdata->n_in_cable <= 0) {
		dev_err(&pdev->dev, "Not found input cable names\n");
		return ERR_PTR(-EINVAL);
	}
	pdata->in_cable_names = devm_kzalloc(&pdev->dev,
				(pdata->n_in_cable + 1) *
				sizeof(*pdata->in_cable_names), GFP_KERNEL);
	if (!pdata->in_cable_names)
		return ERR_PTR(-ENOMEM);
	count = 0;
	of_property_for_each_string(np, "extcon-cable-names", prop, names)
		pdata->in_cable_names[count++] = names;
	pdata->in_cable_names[count] = NULL;

	pdata->n_io_states = of_property_count_u32_elems(np, "cable-states");
	if ((pdata->n_io_states < 3) || (pdata->n_io_states % 3 != 0)) {
		dev_err(&pdev->dev, "not found proper cable state\n");
		goto cable_new_states;
	}
	pdata->n_io_states /= 3;
	pdata->io_states = devm_kzalloc(&pdev->dev, (pdata->n_io_states) *
					sizeof(*pdata->io_states), GFP_KERNEL);
	if (!pdata->io_states)
		return ERR_PTR(-ENOMEM);
	for (count = 0;  count < pdata->n_io_states; ++count) {
		ret = of_property_read_u32_index(np, "cable-states",
						 count * 3, &pval);
		if (!ret)
			pdata->io_states[count].in_state = pval;

		ret = of_property_read_u32_index(np, "cable-states",
						 count * 3 + 1, &pval);
		if (!ret)
			pdata->io_states[count].in_mask = pval;

		ret = of_property_read_u32_index(np, "cable-states",
						 count * 3 + 2, &pval);
		if (!ret)
			pdata->io_states[count].out_states = pval;
	}

cable_new_states:
	pdata->n_io_new_states = of_property_count_u32_elems(np,
							     "cable-new-states");
	if ((pdata->n_io_new_states < 5) || (pdata->n_io_new_states % 5 != 0)) {
		dev_dbg(&pdev->dev, "not found proper cable-new-states\n");
		goto exit;
	}
	pdata->n_io_new_states /= 5;
	pdata->io_new_states = devm_kzalloc(&pdev->dev,
					    (pdata->n_io_new_states) *
					    sizeof(*pdata->io_new_states),
					    GFP_KERNEL);
	if (!pdata->io_new_states)
		return ERR_PTR(-ENOMEM);
	for (count = 0;  count < pdata->n_io_new_states; ++count) {
		ret = of_property_read_u32_index(np, "cable-new-states",
						 count * 5, &pval);
		if (!ret)
			pdata->io_new_states[count].last_cable_in_state = pval;

		ret = of_property_read_u32_index(np, "cable-new-states",
						 count * 5 + 1, &pval);
		if (!ret)
			pdata->io_new_states[count].current_in_cable_state = pval;

		ret = of_property_read_u32_index(np, "cable-new-states",
						 count * 5 + 2, &pval);
		if (!ret)
			pdata->io_new_states[count].in_mask = pval;

		ret = of_property_read_u32_index(np, "cable-new-states",
						 count * 5 + 3, &pval);
		if (!ret)
			pdata->io_new_states[count].new_cable_out_state = pval;

		ret = of_property_read_u32_index(np, "cable-new-states",
						 count * 5 + 4, &pval);
		if (!ret)
			pdata->io_new_states[count].reschedule_wq = pval;
	}
exit:
	if ((pdata->n_io_states < 3) && (pdata->n_io_new_states < 5))
		return ERR_PTR(-EINVAL);
	return pdata;
}

static int ecx_probe(struct platform_device *pdev)
{
	int ret = 0;
	struct extcon_cable_xlate *ecx;
	struct ecx_platform_data *pdata = dev_get_platdata(&pdev->dev);

	if (!pdata && pdev->dev.of_node) {
		pdata = ecx_get_pdata_from_dt(pdev);
		if (IS_ERR(pdata)) {
			pdata = NULL;
		}
	}

	if (!pdata) {
		dev_err(&pdev->dev, "No platform data, exiting..\n");
		return -ENODEV;
	}

	ecx = devm_kzalloc(&pdev->dev, sizeof(*ecx), GFP_KERNEL);
	if (!ecx)
		return -ENOMEM;

	ecx->in_cables = devm_kzalloc(&pdev->dev, pdata->n_in_cable *
				sizeof(*ecx->in_cables), GFP_KERNEL);
	if (!ecx->in_cables)
		return -ENOMEM;

	ecx->dev = &pdev->dev;
	dev_set_drvdata(&pdev->dev, ecx);
	spin_lock_init(&ecx->lock);
	mutex_init(&ecx->cable_lock);

	ecx->edev = devm_extcon_dev_allocate(&pdev->dev,
					     pdata->out_cable_names);
	if (IS_ERR(ecx->edev)) {
		dev_err(&pdev->dev, "failed to allocate extcon device\n");
		return -ENOMEM;
	}
	ecx->edev->name = pdata->name;
	ecx->debounce_jiffies = msecs_to_jiffies(pdata->cable_insert_delay);
	ecx->timer_to_work_jiffies = msecs_to_jiffies(500);
	ecx->pdata = pdata;

	wakeup_source_init(&ecx->wake_lock, "extcon-suspend-lock");

	ret = devm_extcon_dev_register(&pdev->dev, ecx->edev);
	if (ret < 0) {
		dev_err(ecx->dev, "Extcon registration failed: %d\n", ret);
		return ret;
	}

	INIT_DELAYED_WORK(&ecx->work, ecx_cable_state_update_work);
	setup_timer(&ecx->timer, ecx_extcon_notifier_timer, (unsigned long)ecx);

	ret = ecx_init_input_cables(ecx);
	if (ret < 0) {
		if (ret == -EPROBE_DEFER)
			goto defer_now;
		return ret;
	}

	ecx->extcon_init_done = true;

defer_now:
	if (ecx->extcon_init_done) {
		ecx_cable_state_update_work(&ecx->work.work);
	} else {
		extcon_set_state(ecx->edev, EXTCON_NONE, 0);
		schedule_delayed_work(&ecx->work, msecs_to_jiffies(1000));
	}
	dev_info(&pdev->dev, "%s() get success\n", __func__);
	return 0;
}

static int ecx_remove(struct platform_device *pdev)
{
	struct extcon_cable_xlate *ecx = platform_get_drvdata(pdev);

	del_timer_sync(&ecx->timer);
	cancel_delayed_work_sync(&ecx->work);
	return 0;
}

static const struct of_device_id ecx_of_match[] = {
	{ .compatible = "extcon-cable-xlate", },
	{},
};
MODULE_DEVICE_TABLE(of, ecx_of_match);

static struct platform_driver ecx_driver = {
	.driver = {
		.name = "extcon-cable-xlate",
		.owner = THIS_MODULE,
		.of_match_table = ecx_of_match,
	},
	.probe = ecx_probe,
	.remove = ecx_remove,
};

static int __init ecx_init(void)
{
	return platform_driver_register(&ecx_driver);
}

static void __exit ecx_exit(void)
{
	platform_driver_unregister(&ecx_driver);
}

subsys_initcall_sync(ecx_init);
module_exit(ecx_exit);

MODULE_DESCRIPTION("Power supply detection through extcon driver");
MODULE_AUTHOR("Laxman Dewangan <ldewangan@nvidia.com>");
MODULE_LICENSE("GPL v2");