hwmon: (nct6775) Support two SuperIO chips in the same system (698a7c24) · Commits · e / devices / android_kernel_fairphone_FP5

drivers/hwmon/nct6775.c

+63 −50

Original line number	Original line	Diff line number	Diff line
	@@ -3484,11 +3484,11 @@ static const char * const nct6775_sio_names[] __initconst = {
	};		};

	/* nct6775_find() looks for a '627 in the Super-I/O config space */		/* nct6775_find() looks for a '627 in the Super-I/O config space */
	static int __init nct6775_find(int sioaddr, unsigned short *addr,		static int __init nct6775_find(int sioaddr, struct nct6775_sio_data *sio_data)
	struct nct6775_sio_data *sio_data)
	{		{
	u16 val;		u16 val;
	int err;		int err;
			int addr;

	err = superio_enter(sioaddr);		err = superio_enter(sioaddr);
	if (err)		if (err)
	@@ -3520,8 +3520,8 @@ static int __init nct6775_find(int sioaddr, unsigned short *addr,
	superio_select(sioaddr, NCT6775_LD_HWM);		superio_select(sioaddr, NCT6775_LD_HWM);
	val = (superio_inb(sioaddr, SIO_REG_ADDR) << 8)		val = (superio_inb(sioaddr, SIO_REG_ADDR) << 8)
	\| superio_inb(sioaddr, SIO_REG_ADDR + 1);		\| superio_inb(sioaddr, SIO_REG_ADDR + 1);
	*addr = val & IOREGION_ALIGNMENT;		addr = val & IOREGION_ALIGNMENT;
	if (*addr == 0) {		if (addr == 0) {
	pr_err("Refusing to enable a Super-I/O device with a base I/O port 0\n");		pr_err("Refusing to enable a Super-I/O device with a base I/O port 0\n");
	superio_exit(sioaddr);		superio_exit(sioaddr);
	return -ENODEV;		return -ENODEV;
	@@ -3535,11 +3535,11 @@ static int __init nct6775_find(int sioaddr, unsigned short *addr,
	}		}

	superio_exit(sioaddr);		superio_exit(sioaddr);
	pr_info("Found %s or compatible chip at %#x\n",		pr_info("Found %s or compatible chip at %#x:%#x\n",
	nct6775_sio_names[sio_data->kind], *addr);		nct6775_sio_names[sio_data->kind], sioaddr, addr);
	sio_data->sioreg = sioaddr;		sio_data->sioreg = sioaddr;

	return 0;		return addr;
	}		}

	/*		/*
	@@ -3548,14 +3548,20 @@ static int __init nct6775_find(int sioaddr, unsigned short *addr,
	* track of the nct6775 driver. But since we platform_device_alloc(), we		* track of the nct6775 driver. But since we platform_device_alloc(), we
	* must keep track of the device		* must keep track of the device
	*/		*/
	static struct platform_device *pdev;		static struct platform_device *pdev[2];

	static int __init sensors_nct6775_init(void)		static int __init sensors_nct6775_init(void)
	{		{
	int err;		int i, err;
	unsigned short address;		bool found = false;
			int address;
	struct resource res;		struct resource res;
	struct nct6775_sio_data sio_data;		struct nct6775_sio_data sio_data;
			int sioaddr[2] = { 0x2e, 0x4e };

			err = platform_driver_register(&nct6775_driver);
			if (err)
			return err;

	/*		/*
	* initialize sio_data->kind and sio_data->sioreg.		* initialize sio_data->kind and sio_data->sioreg.
	@@ -3564,27 +3570,23 @@ static int __init sensors_nct6775_init(void)
	* driver will probe 0x2e and 0x4e and auto-detect the presence of a		* driver will probe 0x2e and 0x4e and auto-detect the presence of a
	* nct6775 hardware monitor, and call probe()		* nct6775 hardware monitor, and call probe()
	*/		*/
	if (nct6775_find(0x2e, &address, &sio_data) &&		for (i = 0; i < ARRAY_SIZE(pdev); i++) {
	nct6775_find(0x4e, &address, &sio_data))		address = nct6775_find(sioaddr[i], &sio_data);
	return -ENODEV;		if (address <= 0)
			continue;

	err = platform_driver_register(&nct6775_driver);		found = true;
	if (err)
	goto exit;

	pdev = platform_device_alloc(DRVNAME, address);		pdev[i] = platform_device_alloc(DRVNAME, address);
	if (!pdev) {		if (!pdev[i]) {
	err = -ENOMEM;		err = -ENOMEM;
	pr_err("Device allocation failed\n");		goto exit_device_put;
	goto exit_unregister;
	}		}

	err = platform_device_add_data(pdev, &sio_data,		err = platform_device_add_data(pdev[i], &sio_data,
	sizeof(struct nct6775_sio_data));		sizeof(struct nct6775_sio_data));
	if (err) {		if (err)
	pr_err("Platform data allocation failed\n");
	goto exit_device_put;		goto exit_device_put;
	}

	memset(&res, 0, sizeof(res));		memset(&res, 0, sizeof(res));
	res.name = DRVNAME;		res.name = DRVNAME;
	@@ -3593,35 +3595,46 @@ static int __init sensors_nct6775_init(void)
	res.flags = IORESOURCE_IO;		res.flags = IORESOURCE_IO;

	err = acpi_check_resource_conflict(&res);		err = acpi_check_resource_conflict(&res);
	if (err)
	goto exit_device_put;

	err = platform_device_add_resources(pdev, &res, 1);
	if (err) {		if (err) {
	pr_err("Device resource addition failed (%d)\n", err);		platform_device_put(pdev[i]);
	goto exit_device_put;		pdev[i] = NULL;
			continue;
	}		}

			err = platform_device_add_resources(pdev[i], &res, 1);
			if (err)
			goto exit_device_put;

	/* platform_device_add calls probe() */		/* platform_device_add calls probe() */
	err = platform_device_add(pdev);		err = platform_device_add(pdev[i]);
	if (err) {		if (err)
	pr_err("Device addition failed (%d)\n", err);
	goto exit_device_put;		goto exit_device_put;
	}		}
			if (!found) {
			err = -ENODEV;
			goto exit_unregister;
			}

	return 0;		return 0;

	exit_device_put:		exit_device_put:
	platform_device_put(pdev);		for (i = 0; i < ARRAY_SIZE(pdev); i++) {
			if (pdev[i])
			platform_device_put(pdev[i]);
			}
	exit_unregister:		exit_unregister:
	platform_driver_unregister(&nct6775_driver);		platform_driver_unregister(&nct6775_driver);
	exit:
	return err;		return err;
	}		}

	static void __exit sensors_nct6775_exit(void)		static void __exit sensors_nct6775_exit(void)
	{		{
	platform_device_unregister(pdev);		int i;

			for (i = 0; i < ARRAY_SIZE(pdev); i++) {
			if (pdev[i])
			platform_device_unregister(pdev[i]);
			}
	platform_driver_unregister(&nct6775_driver);		platform_driver_unregister(&nct6775_driver);
	}		}