C++ ctrl_err函数代码示例

static int __pciehp_link_set(struct controller *ctrl, bool enable)
{
	u16 lnk_ctrl;
	int retval = 0;

	retval = pciehp_readw(ctrl, PCI_EXP_LNKCTL, &lnk_ctrl);
	if (retval) {
		ctrl_err(ctrl, "Cannot read LNKCTRL register\n");
		return retval;
	}

	if (enable)
		lnk_ctrl &= ~PCI_EXP_LNKCTL_LD;
	else
		lnk_ctrl |= PCI_EXP_LNKCTL_LD;

	retval = pciehp_writew(ctrl, PCI_EXP_LNKCTL, lnk_ctrl);
	if (retval) {
		ctrl_err(ctrl, "Cannot write LNKCTRL register\n");
		return retval;
	}
	ctrl_dbg(ctrl, "%s: lnk_ctrl = %x\n", __func__, lnk_ctrl);

	return retval;
}

static int hpc_check_cmd_status(struct controller *ctrl)
{
	int retval = 0;
	u16 cmd_status = shpc_readw(ctrl, CMD_STATUS) & 0x000F;

	switch (cmd_status >> 1) {
	case 0:
		retval = 0;
		break;
	case 1:
		retval = SWITCH_OPEN;
		ctrl_err(ctrl, "Switch opened!\n");
		break;
	case 2:
		retval = INVALID_CMD;
		ctrl_err(ctrl, "Invalid HPC command!\n");
		break;
	case 4:
		retval = INVALID_SPEED_MODE;
		ctrl_err(ctrl, "Invalid bus speed/mode!\n");
		break;
	default:
		retval = cmd_status;
	}

	return retval;
}

int pciehp_check_link_status(struct controller *ctrl)
{
	u16 lnk_status;
	int retval = 0;

        /*
         * Data Link Layer Link Active Reporting must be capable for
         * hot-plug capable downstream port. But old controller might
         * not implement it. In this case, we wait for 1000 ms.
         */
        if (ctrl->link_active_reporting)
                pcie_wait_link_active(ctrl);
        else
                msleep(1000);

	retval = pciehp_readw(ctrl, PCI_EXP_LNKSTA, &lnk_status);
	if (retval) {
		ctrl_err(ctrl, "Cannot read LNKSTATUS register\n");
		return retval;
	}

	ctrl_dbg(ctrl, "%s: lnk_status = %x\n", __func__, lnk_status);
	if ((lnk_status & PCI_EXP_LNKSTA_LT) ||
	    !(lnk_status & PCI_EXP_LNKSTA_NLW)) {
		ctrl_err(ctrl, "Link Training Error occurs \n");
		retval = -1;
		return retval;
	}

	return retval;
}

static void set_slot_off(struct controller *ctrl, struct slot * pslot)
{
	/* turn off slot, turn on Amber LED, turn off Green LED if supported*/
	if (POWER_CTRL(ctrl)) {
		if (pslot->hpc_ops->power_off_slot(pslot)) {
			ctrl_err(ctrl,
				 "Issue of Slot Power Off command failed\n");
			return;
		}
	}

	/*
	 * After turning power off, we must wait for at least 1 second
	 * before taking any action that relies on power having been
	 * removed from the slot/adapter.
	 */
	msleep(1000);

	if (PWR_LED(ctrl))
		pslot->hpc_ops->green_led_off(pslot);

	if (ATTN_LED(ctrl)) {
		if (pslot->hpc_ops->set_attention_status(pslot, 1)) {
			ctrl_err(ctrl,
				 "Issue of Set Attention Led command failed\n");
			return;
		}
	}
}

/**
 * board_added - Called after a board has been added to the system.
 * @p_slot: &slot where board is added
 *
 * Turns power on for the board.
 * Configures board.
 */
static int board_added(struct slot *p_slot)
{
	int retval = 0;
	struct controller *ctrl = p_slot->ctrl;
	struct pci_bus *parent = ctrl->pci_dev->subordinate;

	ctrl_dbg(ctrl, "%s: slot device, slot offset, hp slot = %d, %d, %d\n",
		 __func__, p_slot->device, ctrl->slot_device_offset,
		 p_slot->hp_slot);

	if (POWER_CTRL(ctrl)) {
		/* Power on slot */
		retval = p_slot->hpc_ops->power_on_slot(p_slot);
		if (retval)
			return retval;
	}

	if (PWR_LED(ctrl))
		p_slot->hpc_ops->green_led_blink(p_slot);

	/* Check link training status */
	retval = p_slot->hpc_ops->check_lnk_status(ctrl);
	if (retval) {
		ctrl_err(ctrl, "Failed to check link status\n");
		set_slot_off(ctrl, p_slot);
		return retval;
	}

	/* Check for a power fault */
	if (p_slot->hpc_ops->query_power_fault(p_slot)) {
		ctrl_dbg(ctrl, "Power fault detected\n");
		retval = POWER_FAILURE;
		goto err_exit;
	}

	retval = pciehp_configure_device(p_slot);
	if (retval) {
		ctrl_err(ctrl, "Cannot add device at %04x:%02x:%02x\n",
			 pci_domain_nr(parent), p_slot->bus, p_slot->device);
		goto err_exit;
	}

	/*
	 * Some PCI Express root ports require fixup after hot-plug operation.
	 */
	if (pcie_mch_quirk)
		pci_fixup_device(pci_fixup_final, ctrl->pci_dev);
	if (PWR_LED(ctrl))
  		p_slot->hpc_ops->green_led_on(p_slot);

	return 0;

err_exit:
	set_slot_off(ctrl, p_slot);
	return retval;
}

/**
 * board_added - Called after a board has been added to the system.
 * @p_slot: &slot where board is added
 *
 * Turns power on for the board.
 * Configures board.
 */
static int board_added(struct slot *p_slot)
{
	int retval = 0;
	struct controller *ctrl = p_slot->ctrl;
	struct pci_bus *parent = ctrl->pcie->port->subordinate;

	if (POWER_CTRL(ctrl)) {
		/* Power on slot */
		retval = pciehp_power_on_slot(p_slot);
		if (retval)
			return retval;
	}

	pciehp_green_led_blink(p_slot);

	/* Check link training status */
	pm_runtime_get_sync(&ctrl->pcie->port->dev);
	retval = pciehp_check_link_status(ctrl);
	if (retval) {
		ctrl_err(ctrl, "Failed to check link status\n");
		goto err_exit;
	}

	/* Check for a power fault */
	if (ctrl->power_fault_detected || pciehp_query_power_fault(p_slot)) {
		ctrl_err(ctrl, "Slot(%s): Power fault\n", slot_name(p_slot));
		retval = -EIO;
		goto err_exit;
	}

	retval = pciehp_configure_device(p_slot);
	if (retval) {
		ctrl_err(ctrl, "Cannot add device at %04x:%02x:00\n",
			 pci_domain_nr(parent), parent->number);
		if (retval != -EEXIST)
			goto err_exit;
	}
	pm_runtime_put(&ctrl->pcie->port->dev);

	pciehp_green_led_on(p_slot);
	pciehp_set_attention_status(p_slot, 0);
	return 0;

err_exit:
	pm_runtime_put(&ctrl->pcie->port->dev);
	set_slot_off(ctrl, p_slot);
	return retval;
}

int pciehp_power_on_slot(struct slot * slot)
{
	struct controller *ctrl = slot->ctrl;
	u16 slot_cmd;
	u16 cmd_mask;
	u16 slot_status;
	u16 lnk_status;
	int retval = 0;

	/* Clear sticky power-fault bit from previous power failures */
	retval = pciehp_readw(ctrl, PCI_EXP_SLTSTA, &slot_status);
	if (retval) {
		ctrl_err(ctrl, "%s: Cannot read SLOTSTATUS register\n",
			 __func__);
		return retval;
	}
	slot_status &= PCI_EXP_SLTSTA_PFD;
	if (slot_status) {
		retval = pciehp_writew(ctrl, PCI_EXP_SLTSTA, slot_status);
		if (retval) {
			ctrl_err(ctrl,
				 "%s: Cannot write to SLOTSTATUS register\n",
				 __func__);
			return retval;
		}
	}
	ctrl->power_fault_detected = 0;

	slot_cmd = POWER_ON;
	cmd_mask = PCI_EXP_SLTCTL_PCC;
	retval = pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
	if (retval) {
		ctrl_err(ctrl, "Write %x command failed!\n", slot_cmd);
		return retval;
	}
	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL, slot_cmd);

	retval = pciehp_readw(ctrl, PCI_EXP_LNKSTA, &lnk_status);
	if (retval) {
		ctrl_err(ctrl, "%s: Cannot read LNKSTA register\n",
				__func__);
		return retval;
	}
	pcie_update_link_speed(ctrl->pcie->port->subordinate, lnk_status);

	return retval;
}

int pciehp_get_cur_link_speed(struct slot *slot, enum pci_bus_speed *value)
{
	struct controller *ctrl = slot->ctrl;
	enum pcie_link_speed lnk_speed = PCI_SPEED_UNKNOWN;
	int retval = 0;
	u16 lnk_status;

	retval = pciehp_readw(ctrl, PCI_EXP_LNKSTA, &lnk_status);
	if (retval) {
		ctrl_err(ctrl, "%s: Cannot read LNKSTATUS register\n",
			 __func__);
		return retval;
	}

	switch (lnk_status & PCI_EXP_LNKSTA_CLS) {
	case 1:
		lnk_speed = PCIE_2_5GB;
		break;
	case 2:
		lnk_speed = PCIE_5_0GB;
		break;
	default:
		lnk_speed = PCIE_LNK_SPEED_UNKNOWN;
		break;
	}

	*value = lnk_speed;
	ctrl_dbg(ctrl, "Current link speed = %d\n", lnk_speed);

	return retval;
}

int pciehp_get_max_link_speed(struct slot *slot, enum pci_bus_speed *value)
{
	struct controller *ctrl = slot->ctrl;
	enum pcie_link_speed lnk_speed;
	u32	lnk_cap;
	int retval = 0;

	retval = pciehp_readl(ctrl, PCI_EXP_LNKCAP, &lnk_cap);
	if (retval) {
		ctrl_err(ctrl, "%s: Cannot read LNKCAP register\n", __func__);
		return retval;
	}

	switch (lnk_cap & 0x000F) {
	case 1:
		lnk_speed = PCIE_2_5GB;
		break;
	case 2:
		lnk_speed = PCIE_5_0GB;
		break;
	default:
		lnk_speed = PCIE_LNK_SPEED_UNKNOWN;
		break;
	}

	*value = lnk_speed;
	ctrl_dbg(ctrl, "Max link speed = %d\n", lnk_speed);

	return retval;
}

int pciehp_power_on_slot(struct slot *slot)
{
	struct controller *ctrl = slot->ctrl;
	struct pci_dev *pdev = ctrl_dev(ctrl);
	u16 slot_status;
	int retval;

	/* Clear sticky power-fault bit from previous power failures */
	pcie_capability_read_word(pdev, PCI_EXP_SLTSTA, &slot_status);
	if (slot_status & PCI_EXP_SLTSTA_PFD)
		pcie_capability_write_word(pdev, PCI_EXP_SLTSTA,
					   PCI_EXP_SLTSTA_PFD);
	ctrl->power_fault_detected = 0;

	pcie_write_cmd(ctrl, PCI_EXP_SLTCTL_PWR_ON, PCI_EXP_SLTCTL_PCC);
	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL,
		 PCI_EXP_SLTCTL_PWR_ON);

	retval = pciehp_link_enable(ctrl);
	if (retval)
		ctrl_err(ctrl, "%s: Can not enable the link!\n", __func__);

	return retval;
}

int pciehp_power_off_slot(struct slot * slot)
{
	struct controller *ctrl = slot->ctrl;
	u16 slot_cmd;
	u16 cmd_mask;
	int retval;

	/* Disable the link at first */
	pciehp_link_disable(ctrl);
	/* wait the link is down */
	if (ctrl->link_active_reporting)
		pcie_wait_link_not_active(ctrl);
	else
		msleep(1000);

	slot_cmd = POWER_OFF;
	cmd_mask = PCI_EXP_SLTCTL_PCC;
	retval = pcie_write_cmd(ctrl, slot_cmd, cmd_mask);
	if (retval) {
		ctrl_err(ctrl, "Write command failed!\n");
		return retval;
	}
	ctrl_dbg(ctrl, "%s: SLOTCTRL %x write cmd %x\n", __func__,
		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL, slot_cmd);
	return 0;
}

int shpchp_unconfigure_device(struct slot *p_slot)
{
	int rc = 0;
	int j;
	u8 bctl = 0;
	struct pci_bus *parent = p_slot->ctrl->pci_dev->subordinate;
	struct controller *ctrl = p_slot->ctrl;

	ctrl_dbg(ctrl, "%s: domain:bus:dev = %04x:%02x:%02x\n",
		 __func__, pci_domain_nr(parent), p_slot->bus, p_slot->device);

	for (j = 0; j < 8 ; j++) {
		struct pci_dev *temp = pci_get_slot(parent,
				(p_slot->device << 3) | j);
		if (!temp)
			continue;
		if (temp->hdr_type == PCI_HEADER_TYPE_BRIDGE) {
			pci_read_config_byte(temp, PCI_BRIDGE_CONTROL, &bctl);
			if (bctl & PCI_BRIDGE_CTL_VGA) {
				ctrl_err(ctrl,
					 "Cannot remove display device %s\n",
					 pci_name(temp));
				pci_dev_put(temp);
				rc = -EINVAL;
				break;
			}
		}
		pci_remove_bus_device(temp);
		pci_dev_put(temp);
	}
	return rc;
}

int pcie_enable_notification(struct controller *ctrl)
{
	u16 cmd, mask;

	/*
	 * TBD: Power fault detected software notification support.
	 *
	 * Power fault detected software notification is not enabled
	 * now, because it caused power fault detected interrupt storm
	 * on some machines. On those machines, power fault detected
	 * bit in the slot status register was set again immediately
	 * when it is cleared in the interrupt service routine, and
	 * next power fault detected interrupt was notified again.
	 */
	cmd = PCI_EXP_SLTCTL_PDCE;
	if (ATTN_BUTTN(ctrl))
		cmd |= PCI_EXP_SLTCTL_ABPE;
	if (MRL_SENS(ctrl))
		cmd |= PCI_EXP_SLTCTL_MRLSCE;
	if (!pciehp_poll_mode)
		cmd |= PCI_EXP_SLTCTL_HPIE | PCI_EXP_SLTCTL_CCIE;

	mask = (PCI_EXP_SLTCTL_PDCE | PCI_EXP_SLTCTL_ABPE |
		PCI_EXP_SLTCTL_MRLSCE | PCI_EXP_SLTCTL_PFDE |
		PCI_EXP_SLTCTL_HPIE | PCI_EXP_SLTCTL_CCIE);

	if (pcie_write_cmd(ctrl, cmd, mask)) {
		ctrl_err(ctrl, "Cannot enable software notification\n");
		return -1;
	}
	return 0;
}

int pciehp_get_power_status(struct slot *slot, u8 *status)
{
	struct controller *ctrl = slot->ctrl;
	u16 slot_ctrl;
	u8 pwr_state;
	int	retval = 0;

	retval = pciehp_readw(ctrl, PCI_EXP_SLTCTL, &slot_ctrl);
	if (retval) {
		ctrl_err(ctrl, "%s: Cannot read SLOTCTRL register\n", __func__);
		return retval;
	}
	ctrl_dbg(ctrl, "%s: SLOTCTRL %x value read %x\n", __func__,
		 pci_pcie_cap(ctrl->pcie->port) + PCI_EXP_SLTCTL, slot_ctrl);

	pwr_state = (slot_ctrl & PCI_EXP_SLTCTL_PCC) >> 10;

	switch (pwr_state) {
	case 0:
		*status = 1;
		break;
	case 1:
		*status = 0;
		break;
	default:
		*status = 0xFF;
		break;
	}

	return retval;
}

/**
 * remove_board - Turns off slot and LEDs
 * @p_slot: slot where board is being removed
 */
static int remove_board(struct slot *p_slot)
{
	int retval = 0;
	struct controller *ctrl = p_slot->ctrl;

	retval = pciehp_unconfigure_device(p_slot);
	if (retval)
		return retval;

	ctrl_dbg(ctrl, "%s: hp_slot = %d\n", __func__, p_slot->hp_slot);

	if (POWER_CTRL(ctrl)) {
		/* power off slot */
		retval = p_slot->hpc_ops->power_off_slot(p_slot);
		if (retval) {
			ctrl_err(ctrl,
				 "Issue of Slot Disable command failed\n");
			return retval;
		}
	}

	/*
	 * After turning power off, we must wait for at least 1 second
	 * before taking any action that relies on power having been
	 * removed from the slot/adapter.
	 */
	msleep(1000);

	if (PWR_LED(ctrl))
		/* turn off Green LED */
		p_slot->hpc_ops->green_led_off(p_slot);

	return 0;
}