3 files changed, 131 insertions, 10 deletions

diff --git a/arch/s390/include/asm/cio.h b/arch/s390/include/asm/cio.h
index b5bfb3123cb1..953a7316b30a 100644
--- a/arch/s390/include/asm/cio.h
+++ b/arch/s390/include/asm/cio.h
@@ -372,6 +372,7 @@ struct gen_pool *cio_gp_dma_create(struct device *dma_dev, int nr_pages);
 /* Function from drivers/s390/cio/chsc.c */
 int chsc_sstpc(void *page, unsigned int op, u16 ctrl, u64 *clock_delta);
 int chsc_sstpi(void *page, void *result, size_t size);
+int chsc_stzi(void *page, void *result, size_t size);
 int chsc_sgib(u32 origin);
 
 #endif
diff --git a/arch/s390/include/asm/stp.h b/arch/s390/include/asm/stp.h
index a5c0bc1e5b90..ba07463897c1 100644
--- a/arch/s390/include/asm/stp.h
+++ b/arch/s390/include/asm/stp.h
@@ -25,7 +25,9 @@ struct stp_irq_parm {
 
 struct stp_sstpi {
 	u32		: 32;
-	u32		:  8;
+	u32 tu		:  1;
+	u32 lu		:  1;
+	u32		:  6;
 	u32 stratum	:  8;
 	u32 vbits	: 16;
 	u32 leaps	: 16;
@@ -46,6 +48,48 @@ struct stp_sstpi {
 	u32 rsvd[48];
 } __packed;
 
+struct stp_tzib {
+	u32 tzan	: 16;
+	u32		: 16;
+	u32 tzo		: 16;
+	u32 dsto	: 16;
+	u32 stn;
+	u32 dstn;
+	u64 dst_on_alg;
+	u64 dst_off_alg;
+} __packed;
+
+struct stp_tcpib {
+	u32 atcode	: 4;
+	u32 ntcode	: 4;
+	u32 d		: 1;
+	u32		: 23;
+	s32 tto;
+	struct stp_tzib atzib;
+	struct stp_tzib ntzib;
+	s32 adst_offset : 16;
+	s32 ndst_offset : 16;
+	u32 rsvd1;
+	u64 ntzib_update;
+	u64 ndsto_update;
+} __packed;
+
+struct stp_lsoib {
+	u32 p		: 1;
+	u32		: 31;
+	s32 also	: 16;
+	s32 nlso	: 16;
+	u64 nlsout;
+} __packed;
+
+struct stp_stzi {
+	u32 rsvd0[3];
+	u64 data_ts;
+	u32 rsvd1[22];
+	struct stp_tcpib tcpib;
+	struct stp_lsoib lsoib;
+} __packed;
+
 /* Functions needed by the machine check handler */
 int stp_sync_check(void);
 int stp_island_check(void);
diff --git a/arch/s390/kernel/time.c b/arch/s390/kernel/time.c
index bee380340bad..49648d574b35 100644
--- a/arch/s390/kernel/time.c
+++ b/arch/s390/kernel/time.c
@@ -598,6 +598,81 @@ static int stp_sync_clock(void *data)
 	return 0;
 }
 
+static int stp_clear_leap(void)
+{
+	struct __kernel_timex txc;
+	int ret;
+
+	memset(&txc, 0, sizeof(txc));
+
+	ret = do_adjtimex(&txc);
+	if (ret < 0)
+		return ret;
+
+	txc.modes = ADJ_STATUS;
+	txc.status &= ~(STA_INS|STA_DEL);
+	return do_adjtimex(&txc);
+}
+
+static void stp_check_leap(void)
+{
+	struct stp_stzi stzi;
+	struct stp_lsoib *lsoib = &stzi.lsoib;
+	struct __kernel_timex txc;
+	int64_t timediff;
+	int leapdiff, ret;
+
+	if (!stp_info.lu || !check_sync_clock()) {
+		/*
+		 * Either a scheduled leap second was removed by the operator,
+		 * or STP is out of sync. In both cases, clear the leap second
+		 * kernel flags.
+		 */
+		if (stp_clear_leap() < 0)
+			pr_err("failed to clear leap second flags\n");
+		return;
+	}
+
+	if (chsc_stzi(stp_page, &stzi, sizeof(stzi))) {
+		pr_err("stzi failed\n");
+		return;
+	}
+
+	timediff = tod_to_ns(lsoib->nlsout - get_tod_clock()) / NSEC_PER_SEC;
+	leapdiff = lsoib->nlso - lsoib->also;
+
+	if (leapdiff != 1 && leapdiff != -1) {
+		pr_err("Cannot schedule %d leap seconds\n", leapdiff);
+		return;
+	}
+
+	if (timediff < 0) {
+		if (stp_clear_leap() < 0)
+			pr_err("failed to clear leap second flags\n");
+	} else if (timediff < 7200) {
+		memset(&txc, 0, sizeof(txc));
+		ret = do_adjtimex(&txc);
+		if (ret < 0)
+			return;
+
+		txc.modes = ADJ_STATUS;
+		if (leapdiff > 0)
+			txc.status |= STA_INS;
+		else
+			txc.status |= STA_DEL;
+		ret = do_adjtimex(&txc);
+		if (ret < 0)
+			pr_err("failed to set leap second flags\n");
+		/* arm Timer to clear leap second flags */
+		mod_timer(&stp_timer, jiffies + msecs_to_jiffies(14400 * MSEC_PER_SEC));
+	} else {
+		/* The day the leap second is scheduled for hasn't been reached. Retry
+		 * in one hour.
+		 */
+		mod_timer(&stp_timer, jiffies + msecs_to_jiffies(3600 * MSEC_PER_SEC));
+	}
+}
+
 /*
  * STP work. Check for the STP state and take over the clock
  * synchronization if the STP clock source is usable.
@@ -616,7 +691,7 @@ static void stp_work_fn(struct work_struct *work)
 		goto out_unlock;
 	}
 
-	rc = chsc_sstpc(stp_page, STP_OP_CTRL, 0xb0e0, NULL);
+	rc = chsc_sstpc(stp_page, STP_OP_CTRL, 0xf0e0, NULL);
 	if (rc)
 		goto out_unlock;
 
@@ -625,14 +700,13 @@ static void stp_work_fn(struct work_struct *work)
 		goto out_unlock;
 
 	/* Skip synchronization if the clock is already in sync. */
-	if (check_sync_clock())
-		goto out_unlock;
-
-	memset(&stp_sync, 0, sizeof(stp_sync));
-	cpus_read_lock();
-	atomic_set(&stp_sync.cpus, num_online_cpus() - 1);
-	stop_machine_cpuslocked(stp_sync_clock, &stp_sync, cpu_online_mask);
-	cpus_read_unlock();
+	if (!check_sync_clock()) {
+		memset(&stp_sync, 0, sizeof(stp_sync));
+		cpus_read_lock();
+		atomic_set(&stp_sync.cpus, num_online_cpus() - 1);
+		stop_machine_cpuslocked(stp_sync_clock, &stp_sync, cpu_online_mask);
+		cpus_read_unlock();
+	}
 
 	if (!check_sync_clock())
 		/*
@@ -640,6 +714,8 @@ static void stp_work_fn(struct work_struct *work)
 		 * Retry after a second.
 		 */
 		mod_timer(&stp_timer, jiffies + msecs_to_jiffies(MSEC_PER_SEC));
+	else if (stp_info.lu)
+		stp_check_leap();
 
 out_unlock:
 	mutex_unlock(&stp_work_mutex);