[RFC PATCH v2 03/22] PCI/IDE: Init IDs on all IDE streams beforehand

[Alexey Kardashevskiy <aik@amd.com>]

The PCIe spec defines two types of streams - selective and link.
Each stream has an ID from the same bucket so a stream ID does not
tell the type.
The spec defines an "enable" bit for every stream and required
stream IDs to be unique among all enabled stream but there is no such
requirement for disabled streams.

However, when IDE_KM is programming keys, an IDE-capable device needs
to know the type of stream being programmed to write it directly to
the hardware as keys are relatively large, possibly many of them and
devices often struggle with keeping around rather big data not being
used.

Walk through all streams on a device and initialize the IDs to some
unique number, both link and selective.

Probably should be a quirk if it turns out not to be a common issue.

Signed-off-by: Alexey Kardashevskiy <aik@amd.com>
---
 drivers/pci/ide.c | 29 ++++++++++++++++++--
 1 file changed, 26 insertions(+), 3 deletions(-)

diff --git a/drivers/pci/ide.c b/drivers/pci/ide.c
index 3c53b27f8447..5f1d5385d3a8 100644
--- a/drivers/pci/ide.c
+++ b/drivers/pci/ide.c
@@ -18,7 +18,7 @@ static int sel_ide_offset(u16 cap, int stream_id, int nr_ide_mem)
 void pci_ide_init(struct pci_dev *pdev)
 {
 	u16 ide_cap, sel_ide_cap;
-	int nr_ide_mem = 0;
+	int nr_ide_mem = 0, i, link_num, sel_num, offset;
 	u32 val = 0;
 
 	if (!pci_is_pcie(pdev))
@@ -33,6 +33,7 @@ void pci_ide_init(struct pci_dev *pdev)
 	 * require consistent number of address association blocks
 	 */
 	pci_read_config_dword(pdev, ide_cap + PCI_IDE_CAP, &val);
+
 	if ((val & PCI_IDE_CAP_SELECTIVE) == 0)
 		return;
 
@@ -43,6 +44,9 @@ void pci_ide_init(struct pci_dev *pdev)
 			return;
 	}
 
+	link_num = PCI_IDE_CAP_LINK_TC_NUM(val) + 1;
+	sel_num = PCI_IDE_CAP_SELECTIVE_STREAMS_NUM(val) + 1;
+
 	if (val & PCI_IDE_CAP_LINK)
 		sel_ide_cap = ide_cap + PCI_IDE_LINK_STREAM +
 			      (PCI_IDE_CAP_LINK_TC_NUM(val) + 1) *
@@ -50,12 +54,13 @@ void pci_ide_init(struct pci_dev *pdev)
 	else
 		sel_ide_cap = ide_cap + PCI_IDE_LINK_STREAM;
 
-	for (int i = 0; i < PCI_IDE_CAP_SELECTIVE_STREAMS_NUM(val) + 1; i++) {
+	for (i = 0; i < PCI_IDE_CAP_SELECTIVE_STREAMS_NUM(val) + 1; i++) {
 		if (i == 0) {
+			offset = 0;
 			pci_read_config_dword(pdev, sel_ide_cap, &val);
 			nr_ide_mem = PCI_IDE_SEL_CAP_ASSOC_NUM(val) + 1;
 		} else {
-			int offset = sel_ide_offset(sel_ide_cap, i, nr_ide_mem);
+			offset = sel_ide_offset(sel_ide_cap, i, nr_ide_mem);
 
 			pci_read_config_dword(pdev, offset, &val);
 
@@ -68,6 +73,24 @@ void pci_ide_init(struct pci_dev *pdev)
 				return;
 			}
 		}
+
+		/* Some devices insist on streamid to be unique even for not enabled streams */
+		val &= ~PCI_IDE_SEL_CTL_ID_MASK;
+		val |= FIELD_PREP(PCI_IDE_SEL_CTL_ID_MASK, i);
+		pci_write_config_dword(pdev, offset + PCI_IDE_SEL_CTL, val);
+	}
+
+	if (val & PCI_IDE_CAP_LINK) {
+		/* Some devices insist on streamid to be unique even for not enabled streams */
+		for (i = 0; i < link_num; ++i) {
+			offset = ide_cap + PCI_IDE_LINK_STREAM + i * PCI_IDE_LINK_BLOCK_SIZE;
+
+			pci_read_config_dword(pdev, offset, &val);
+			val &= ~PCI_IDE_LINK_CTL_ID_MASK;
+			val |= FIELD_PREP(PCI_IDE_LINK_CTL_ID_MASK, i + sel_num);
+
+			pci_write_config_dword(pdev, offset, val);
+		}
 	}
 
 	pdev->ide_cap = ide_cap;
-- 
2.47.1