linux驱动添加延迟队列monitor(Add delay worker for gmsl)

通过delay work实现在linux驱动中添加周期处理函数（monitor）

 

#include <linux/delay.h>
#include <linux/gpio/consumer.h>
#include <linux/i2c.h>
#include <linux/interrupt.h>
#include <linux/irq.h>
#include <linux/kernel.h>
#include <linux/mfd/core.h>
#include <linux/module.h>
#include <linux/of_device.h>
#include <linux/of_irq.h>
#include <linux/regmap.h>
#include <linux/regulator/consumer.h>

#ifdef CONFIG_DIAG_CLASS
#include <linux/diagnostic.h>
#include <linux/diagnostic/diagnostic_gmsl.h>
#endif /* CONFIG_DIAG_CLASS */

struct gmsl_device;
struct gmsl_serdes_data;

struct gmsl_serdes_platform_data {
	const char				*name;
	const u8				id;
	const struct mfd_cell			*mfd;
	const int				mfd_size;
	const u8				nb_remote;
	const u8				nb_irq;
	const struct gmsl_serdes_irq		*irq;
	const int				cross_size;
	const int				cross_addr;
};

struct gmsl_monitor {
	const char *name;
	struct workqueue_struct *workq;
	struct delayed_work reader; /* Used to poll link */
	unsigned long interval;
	int (*gmsl_monitor)(struct gmsl_device *);
	struct gmsl_device	 *gd;
};

struct gmsl_device {
	struct i2c_client *client;
	struct regmap *regmap;

	struct gmsl_monitor *monitor;
	struct gmsl_monitor *irq_monitor;
};

struct gmsl_serdes_data {
	/* local device fields */
	struct i2c_client			*client;
	struct device				*dev;
	struct regmap				*regmap;

	const struct gmsl_serdes_platform_data	*pdata;

	struct gmsl_device gd;

};

static struct gmsl_monitor gmsl_monitor_default = {
	.name = "gms_diag_monitor",
	.interval = 2 * HZ,
	.gmsl_monitor = gmsl_line_fault_monitor_polling,
};

static int gmsl_line_fault_monitor_polling(struct gmsl_device *gd)
{
	struct gmsl_serdes_data *gmsl = i2c_get_clientdata(gd->client);
	return 0;
}

static int gmsl_err_irq_monitor(struct gmsl_device *gd)
{
	struct gmsl_serdes_data *gmsl = i2c_get_clientdata(gd->client);
	return 0;
}

void gmsl_trigger_monitor(struct gmsl_monitor *monitor)
{
	queue_delayed_work(monitor->workq, &monitor->reader,
				monitor->interval);
}

static void gmsl_diag_monitor(struct work_struct *work)
{
	struct gmsl_monitor *monitor = container_of(work,struct gmsl_monitor,reader.work);

	/* if link still not established, then continue polling */
	if (monitor->gmsl_monitor(monitor->gd))
		gmsl_trigger_monitor(monitor);
}

static int gmsl_configure_monitor(struct gmsl_monitor *monitor)
{
	if (!monitor || !monitor->gmsl_monitor)
		return -EINVAL;

	monitor->workq = alloc_ordered_workqueue(monitor->name, 0);
	if (monitor->workq == NULL) {
		printk("Failed to create gmsl monitor workqueue\n");
		return -EINVAL;
	}

	/* setup polling interval */
	if (monitor->interval == 0)
		monitor->interval = 1 * HZ;

	INIT_DELAYED_WORK(&monitor->reader, gmsl_diag_monitor);
	return 0;
}

int gmsl_serdes_diag_init(struct gmsl_serdes_data *gmsl)
{
	int status = 0, irq_count, i, j;
	struct resource *res;
	struct i2c_client *client = gmsl->client;
	int err;

	devm_gmsl_alloc_monitor(&gmsl->gd, &gmsl->gd.monitor);

	memcpy(gmsl->gd.monitor, &gmsl_monitor_default,
		sizeof(gmsl_monitor_default));

	gmsl->gd.monitor->gd = &gmsl->gd;
	gmsl->gd.monitor->interval = msecs_to_jiffies(200);

	/* create irq monitor timeout */
	err = devm_gmsl_alloc_monitor(&gmsl->gd, &gmsl->gd.irq_monitor);
	if (err){
		dev_err(gmsl->dev, "can't create irq monitor timeout\n");
	}

	/*  irq monitor timeout data init */
	gmsl->gd.irq_monitor->name = "gmsl_irq_monitor";
	gmsl->gd.irq_monitor->interval = msecs_to_jiffies(500);
	gmsl->gd.irq_monitor->gd = &gmsl->gd;
	gmsl->gd.irq_monitor->gmsl_monitor = gmsl_err_irq_monitor;
	err = gmsl_configure_monitor(gmsl->gd.irq_monitor);
	if (err){
		dev_err(gmsl->dev, "irq monitor timeout data init failed\n");
	}



	if( gmsl->lflt_monitor ){
		gmsl_configure_monitor(gmsl->gd.monitor);
		gmsl_trigger_monitor(gmsl->gd.monitor);
	}
}


static int gmsl_serdes_probe(struct i2c_client *client,
			     const struct i2c_device_id *id)
{
	const struct of_device_id *of_dev_id;
	struct gmsl_serdes_data *gmsl;
	struct device *dev = &client->dev;
	int status;

	gmsl = devm_kzalloc(dev, sizeof(*gmsl), GFP_KERNEL);
	if (!gmsl)
		return -ENOMEM;

	gmsl->dev = dev;
	gmsl->client = client;
	i2c_set_clientdata(client, gmsl);

	of_dev_id = of_match_device(gmsl_serdes_dt_ids, dev);
	if (!of_dev_id) {
		dev_err(dev, "unable to match device\n");
		status = -EINVAL;
		goto fail;
	}
	gmsl->pdata = of_dev_id->data;
	dev_info(dev, "probing %s as local device\n", gmsl->pdata->name);

	gmsl->gd.client = gmsl->client;
	gmsl->gd.regmap = gmsl->regmap;


	status = gmsl_serdes_diag_init(gmsl);
	if (status) {
		dev_err(dev, "unable to initialize serdes diag correctly\n");
		goto fail_client;
	}
}