static int ata_eh_set_lpm()

in libata-eh.c [3237:3352]

• 82 lines of code
• 34 McCabe index (conditional complexity)

static int ata_eh_set_lpm(struct ata_link *link, enum ata_lpm_policy policy,
			  struct ata_device **r_failed_dev)
{
	struct ata_port *ap = ata_is_host_link(link) ? link->ap : NULL;
	struct ata_eh_context *ehc = &link->eh_context;
	struct ata_device *dev, *link_dev = NULL, *lpm_dev = NULL;
	enum ata_lpm_policy old_policy = link->lpm_policy;
	bool no_dipm = link->ap->flags & ATA_FLAG_NO_DIPM;
	unsigned int hints = ATA_LPM_EMPTY | ATA_LPM_HIPM;
	unsigned int err_mask;
	int rc;

	/* if the link or host doesn't do LPM, noop */
	if (!IS_ENABLED(CONFIG_SATA_HOST) ||
	    (link->flags & ATA_LFLAG_NO_LPM) || (ap && !ap->ops->set_lpm))
		return 0;

	/*
	 * DIPM is enabled only for MIN_POWER as some devices
	 * misbehave when the host NACKs transition to SLUMBER.  Order
	 * device and link configurations such that the host always
	 * allows DIPM requests.
	 */
	ata_for_each_dev(dev, link, ENABLED) {
		bool hipm = ata_id_has_hipm(dev->id);
		bool dipm = ata_id_has_dipm(dev->id) && !no_dipm;

		/* find the first enabled and LPM enabled devices */
		if (!link_dev)
			link_dev = dev;

		if (!lpm_dev && (hipm || dipm))
			lpm_dev = dev;

		hints &= ~ATA_LPM_EMPTY;
		if (!hipm)
			hints &= ~ATA_LPM_HIPM;

		/* disable DIPM before changing link config */
		if (policy < ATA_LPM_MED_POWER_WITH_DIPM && dipm) {
			err_mask = ata_dev_set_feature(dev,
					SETFEATURES_SATA_DISABLE, SATA_DIPM);
			if (err_mask && err_mask != AC_ERR_DEV) {
				ata_dev_warn(dev,
					     "failed to disable DIPM, Emask 0x%x\n",
					     err_mask);
				rc = -EIO;
				goto fail;
			}
		}
	}

	if (ap) {
		rc = ap->ops->set_lpm(link, policy, hints);
		if (!rc && ap->slave_link)
			rc = ap->ops->set_lpm(ap->slave_link, policy, hints);
	} else
		rc = sata_pmp_set_lpm(link, policy, hints);

	/*
	 * Attribute link config failure to the first (LPM) enabled
	 * device on the link.
	 */
	if (rc) {
		if (rc == -EOPNOTSUPP) {
			link->flags |= ATA_LFLAG_NO_LPM;
			return 0;
		}
		dev = lpm_dev ? lpm_dev : link_dev;
		goto fail;
	}

	/*
	 * Low level driver acked the transition.  Issue DIPM command
	 * with the new policy set.
	 */
	link->lpm_policy = policy;
	if (ap && ap->slave_link)
		ap->slave_link->lpm_policy = policy;

	/* host config updated, enable DIPM if transitioning to MIN_POWER */
	ata_for_each_dev(dev, link, ENABLED) {
		if (policy >= ATA_LPM_MED_POWER_WITH_DIPM && !no_dipm &&
		    ata_id_has_dipm(dev->id)) {
			err_mask = ata_dev_set_feature(dev,
					SETFEATURES_SATA_ENABLE, SATA_DIPM);
			if (err_mask && err_mask != AC_ERR_DEV) {
				ata_dev_warn(dev,
					"failed to enable DIPM, Emask 0x%x\n",
					err_mask);
				rc = -EIO;
				goto fail;
			}
		}
	}

	link->last_lpm_change = jiffies;
	link->flags |= ATA_LFLAG_CHANGED;

	return 0;

fail:
	/* restore the old policy */
	link->lpm_policy = old_policy;
	if (ap && ap->slave_link)
		ap->slave_link->lpm_policy = old_policy;

	/* if no device or only one more chance is left, disable LPM */
	if (!dev || ehc->tries[dev->devno] <= 2) {
		ata_link_warn(link, "disabling LPM on the link\n");
		link->flags |= ATA_LFLAG_NO_LPM;
	}
	if (r_failed_dev)
		*r_failed_dev = dev;
	return rc;
}