s390/mem_detect: introduce SCLP storage info

SCLP storage info allows to detect continuous and non-continuous online
memory under LPAR, z/VM and KVM, when standby memory is defined.

Reviewed-by: Heiko Carstens <heiko.carstens@de.ibm.com>
Reviewed-by: Martin Schwidefsky <schwidefsky@de.ibm.com>
Signed-off-by: Vasily Gorbik <gor@linux.ibm.com>
Signed-off-by: Martin Schwidefsky <schwidefsky@de.ibm.com>
diff --git a/arch/s390/boot/mem_detect.c b/arch/s390/boot/mem_detect.c
index 920e6fe..8974e3d 100644
--- a/arch/s390/boot/mem_detect.c
+++ b/arch/s390/boot/mem_detect.c
@@ -126,6 +126,12 @@ void detect_memory(void)
 	unsigned long rzm;
 
 	sclp_early_get_meminfo(&max_physmem_end, &rzm);
+
+	if (!sclp_early_read_storage_info()) {
+		mem_detect.info_source = MEM_DETECT_SCLP_STOR_INFO;
+		return;
+	}
+
 	scan_memory(rzm);
 	mem_detect.info_source = MEM_DETECT_TPROT_LOOP;
 	if (!max_physmem_end)
diff --git a/arch/s390/include/asm/mem_detect.h b/arch/s390/include/asm/mem_detect.h
index 8586ade..00426c0 100644
--- a/arch/s390/include/asm/mem_detect.h
+++ b/arch/s390/include/asm/mem_detect.h
@@ -6,6 +6,7 @@
 
 enum mem_info_source {
 	MEM_DETECT_NONE = 0,
+	MEM_DETECT_SCLP_STOR_INFO,
 	MEM_DETECT_TPROT_LOOP
 };
 
@@ -32,6 +33,8 @@ struct mem_detect_info {
 };
 extern struct mem_detect_info mem_detect;
 
+void add_mem_detect_block(u64 start, u64 end);
+
 static inline int __get_mem_detect_block(u32 n, unsigned long *start,
 					 unsigned long *end)
 {
diff --git a/arch/s390/include/asm/sclp.h b/arch/s390/include/asm/sclp.h
index c21a8b6..e0da13c 100644
--- a/arch/s390/include/asm/sclp.h
+++ b/arch/s390/include/asm/sclp.h
@@ -106,6 +106,7 @@ struct zpci_report_error_header {
 } __packed;
 
 int sclp_early_read_info(void);
+int sclp_early_read_storage_info(void);
 int sclp_early_get_core_info(struct sclp_core_info *info);
 void sclp_early_get_ipl_info(struct sclp_ipl_info *info);
 void sclp_early_detect(void);
diff --git a/drivers/s390/char/sclp.h b/drivers/s390/char/sclp.h
index ffe72f0..b3fcc24 100644
--- a/drivers/s390/char/sclp.h
+++ b/drivers/s390/char/sclp.h
@@ -64,6 +64,7 @@ typedef unsigned int sclp_cmdw_t;
 
 #define SCLP_CMDW_READ_CPU_INFO		0x00010001
 #define SCLP_CMDW_READ_SCP_INFO		0x00020001
+#define SCLP_CMDW_READ_STORAGE_INFO	0x00040001
 #define SCLP_CMDW_READ_SCP_INFO_FORCED	0x00120001
 #define SCLP_CMDW_READ_EVENT_DATA	0x00770005
 #define SCLP_CMDW_WRITE_EVENT_DATA	0x00760005
@@ -197,6 +198,15 @@ struct read_info_sccb {
 	u8	_pad_128[4096 - 128];	/* 128-4095 */
 } __packed __aligned(PAGE_SIZE);
 
+struct read_storage_sccb {
+	struct sccb_header header;
+	u16 max_id;
+	u16 assigned;
+	u16 standby;
+	u16 :16;
+	u32 entries[0];
+} __packed;
+
 static inline void sclp_fill_core_info(struct sclp_core_info *info,
 				       struct read_cpu_info_sccb *sccb)
 {
diff --git a/drivers/s390/char/sclp_cmd.c b/drivers/s390/char/sclp_cmd.c
index d7686a6..37d42de 100644
--- a/drivers/s390/char/sclp_cmd.c
+++ b/drivers/s390/char/sclp_cmd.c
@@ -460,15 +460,6 @@ static int sclp_mem_freeze(struct device *dev)
 	return -EPERM;
 }
 
-struct read_storage_sccb {
-	struct sccb_header header;
-	u16 max_id;
-	u16 assigned;
-	u16 standby;
-	u16 :16;
-	u32 entries[0];
-} __packed;
-
 static const struct dev_pm_ops sclp_mem_pm_ops = {
 	.freeze		= sclp_mem_freeze,
 };
@@ -498,7 +489,7 @@ static int __init sclp_detect_standby_memory(void)
 	for (id = 0; id <= sclp_max_storage_id; id++) {
 		memset(sccb, 0, PAGE_SIZE);
 		sccb->header.length = PAGE_SIZE;
-		rc = sclp_sync_request(0x00040001 | id << 8, sccb);
+		rc = sclp_sync_request(SCLP_CMDW_READ_STORAGE_INFO | id << 8, sccb);
 		if (rc)
 			goto out;
 		switch (sccb->header.response_code) {
diff --git a/drivers/s390/char/sclp_early_core.c b/drivers/s390/char/sclp_early_core.c
index 4f04ba6..0df08dc 100644
--- a/drivers/s390/char/sclp_early_core.c
+++ b/drivers/s390/char/sclp_early_core.c
@@ -10,6 +10,7 @@
 #include <asm/ebcdic.h>
 #include <asm/irq.h>
 #include <asm/sections.h>
+#include <asm/mem_detect.h>
 #include "sclp.h"
 #include "sclp_rw.h"
 
@@ -287,3 +288,55 @@ int __init sclp_early_get_meminfo(unsigned long *mem, unsigned long *rzm)
 	*rzm = rnsize;
 	return 0;
 }
+
+#define SCLP_STORAGE_INFO_FACILITY     0x0000400000000000UL
+
+void __weak __init add_mem_detect_block(u64 start, u64 end) {}
+int __init sclp_early_read_storage_info(void)
+{
+	struct read_storage_sccb *sccb = (struct read_storage_sccb *)&sclp_early_sccb;
+	int rc, id, max_id = 0;
+	unsigned long rn, rzm;
+	sclp_cmdw_t command;
+	u16 sn;
+
+	if (!sclp_info_sccb_valid)
+		return -EIO;
+
+	if (!(sclp_info_sccb.facilities & SCLP_STORAGE_INFO_FACILITY))
+		return -EOPNOTSUPP;
+
+	rzm = sclp_info_sccb.rnsize ?: sclp_info_sccb.rnsize2;
+	rzm <<= 20;
+
+	for (id = 0; id <= max_id; id++) {
+		memset(sclp_early_sccb, 0, sizeof(sclp_early_sccb));
+		sccb->header.length = sizeof(sclp_early_sccb);
+		command = SCLP_CMDW_READ_STORAGE_INFO | (id << 8);
+		rc = sclp_early_cmd(command, sccb);
+		if (rc)
+			goto fail;
+
+		max_id = sccb->max_id;
+		switch (sccb->header.response_code) {
+		case 0x0010:
+			for (sn = 0; sn < sccb->assigned; sn++) {
+				if (!sccb->entries[sn])
+					continue;
+				rn = sccb->entries[sn] >> 16;
+				add_mem_detect_block((rn - 1) * rzm, rn * rzm);
+			}
+			break;
+		case 0x0310:
+		case 0x0410:
+			break;
+		default:
+			goto fail;
+		}
+	}
+
+	return 0;
+fail:
+	mem_detect.count = 0;
+	return -EIO;
+}