static int fsi_slave_init()

in fsi-core.c [985:1120]

• 99 lines of code
• 16 McCabe index (conditional complexity)

static int fsi_slave_init(struct fsi_master *master, int link, uint8_t id)
{
	uint32_t cfam_id;
	struct fsi_slave *slave;
	uint8_t crc;
	__be32 data, llmode, slbus;
	int rc;

	/* Currently, we only support single slaves on a link, and use the
	 * full 23-bit address range
	 */
	if (id != 0)
		return -EINVAL;

	rc = fsi_master_read(master, link, id, 0, &data, sizeof(data));
	if (rc) {
		dev_dbg(&master->dev, "can't read slave %02x:%02x %d\n",
				link, id, rc);
		return -ENODEV;
	}
	cfam_id = be32_to_cpu(data);

	crc = crc4(0, cfam_id, 32);
	if (crc) {
		dev_warn(&master->dev, "slave %02x:%02x invalid cfam id CRC!\n",
				link, id);
		return -EIO;
	}

	dev_dbg(&master->dev, "fsi: found chip %08x at %02x:%02x:%02x\n",
			cfam_id, master->idx, link, id);

	/* If we're behind a master that doesn't provide a self-running bus
	 * clock, put the slave into async mode
	 */
	if (master->flags & FSI_MASTER_FLAG_SWCLOCK) {
		llmode = cpu_to_be32(FSI_LLMODE_ASYNC);
		rc = fsi_master_write(master, link, id,
				FSI_SLAVE_BASE + FSI_LLMODE,
				&llmode, sizeof(llmode));
		if (rc)
			dev_warn(&master->dev,
				"can't set llmode on slave:%02x:%02x %d\n",
				link, id, rc);
	}

	/* We can communicate with a slave; create the slave device and
	 * register.
	 */
	slave = kzalloc(sizeof(*slave), GFP_KERNEL);
	if (!slave)
		return -ENOMEM;

	dev_set_name(&slave->dev, "slave@%02x:%02x", link, id);
	slave->dev.type = &cfam_type;
	slave->dev.parent = &master->dev;
	slave->dev.of_node = fsi_slave_find_of_node(master, link, id);
	slave->dev.release = fsi_slave_release;
	device_initialize(&slave->dev);
	slave->cfam_id = cfam_id;
	slave->master = master;
	slave->link = link;
	slave->id = id;
	slave->size = FSI_SLAVE_SIZE_23b;
	slave->t_send_delay = 16;
	slave->t_echo_delay = 16;

	/* Get chip ID if any */
	slave->chip_id = -1;
	if (slave->dev.of_node) {
		uint32_t prop;
		if (!of_property_read_u32(slave->dev.of_node, "chip-id", &prop))
			slave->chip_id = prop;

	}

	slbus = cpu_to_be32(FSI_SLBUS_FORCE);
	rc = fsi_master_write(master, link, id, FSI_SLAVE_BASE + FSI_SLBUS,
			      &slbus, sizeof(slbus));
	if (rc)
		dev_warn(&master->dev,
			 "can't set slbus on slave:%02x:%02x %d\n", link, id,
			 rc);

	rc = fsi_slave_set_smode(slave);
	if (rc) {
		dev_warn(&master->dev,
				"can't set smode on slave:%02x:%02x %d\n",
				link, id, rc);
		goto err_free;
	}

	/* Allocate a minor in the FSI space */
	rc = __fsi_get_new_minor(slave, fsi_dev_cfam, &slave->dev.devt,
				 &slave->cdev_idx);
	if (rc)
		goto err_free;

	/* Create chardev for userspace access */
	cdev_init(&slave->cdev, &cfam_fops);
	rc = cdev_device_add(&slave->cdev, &slave->dev);
	if (rc) {
		dev_err(&slave->dev, "Error %d creating slave device\n", rc);
		goto err_free_ida;
	}

	/* Now that we have the cdev registered with the core, any fatal
	 * failures beyond this point will need to clean up through
	 * cdev_device_del(). Fortunately though, nothing past here is fatal.
	 */

	if (master->link_config)
		master->link_config(master, link,
				    slave->t_send_delay,
				    slave->t_echo_delay);

	/* Legacy raw file -> to be removed */
	rc = device_create_bin_file(&slave->dev, &fsi_slave_raw_attr);
	if (rc)
		dev_warn(&slave->dev, "failed to create raw attr: %d\n", rc);


	rc = fsi_slave_scan(slave);
	if (rc)
		dev_dbg(&master->dev, "failed during slave scan with: %d\n",
				rc);

	return 0;

err_free_ida:
	fsi_free_minor(slave->dev.devt);
err_free:
	of_node_put(slave->dev.of_node);
	kfree(slave);
	return rc;
}