[v3,3/3] SBE: Add timer support

diff --git a/core/timer.c b/core/timer.c
index 7548996..b2a3334 100644
--- a/core/timer.c
+++ b/core/timer.c
@@ -4,6 +4,7 @@ 
 #include <fsp.h>
 #include <device.h>
 #include <opal.h>
+#include <sbe.h>
 
 #ifdef __TEST__
 #define this_cpu()	((void *)-1)
@@ -105,8 +106,12 @@  static void __schedule_timer_at(struct timer *t, uint64_t when)
  bail:
 	/* Pick up the next timer and upddate the SBE HW timer */
 	lt = list_top(&timer_list, struct timer, link);
-	if (lt)
-		slw_update_timer_expiry(lt->target);
+	if (lt) {
+		if (proc_gen < proc_gen_p9)
+			slw_update_timer_expiry(lt->target);
+		else
+			sbe_update_timer_expiry(lt->target);
+	}
 }
 
 void schedule_timer_at(struct timer *t, uint64_t when)
@@ -163,7 +168,10 @@  static void __check_poll_timers(uint64_t now)
 		 * arbitrarily 1us.
 		 */
 		if (t->running) {
-			slw_update_timer_expiry(now + usecs_to_tb(1));
+			if (proc_gen < proc_gen_p9)
+				slw_update_timer_expiry(now + usecs_to_tb(1));
+			else
+				sbe_update_timer_expiry(now + usecs_to_tb(1));
 			break;
 		}
 
@@ -261,6 +269,8 @@  void late_init_timers(void)
 	 */
 	if (platform.heartbeat_time) {
 		heartbeat = platform.heartbeat_time();
+	} else if (sbe_timer_ok()) {
+		heartbeat = HEARTBEAT_DEFAULT_MS * 10;
 	} else if (slw_timer_ok() || fsp_present()) {
 		heartbeat = HEARTBEAT_DEFAULT_MS * 10;
 	}
diff --git a/hw/sbe.c b/hw/sbe.c
index f413179..d3e55b1 100644
--- a/hw/sbe.c
+++ b/hw/sbe.c
@@ -23,6 +23,7 @@ 
 #include <sbe.h>
 #include <skiboot.h>
 #include <timebase.h>
+#include <timer.h>
 #include <trace.h>
 #include <xscom.h>
 
@@ -48,6 +49,25 @@  struct sbe {
 /* Default SBE chip ID */
 static u32 sbe_default_chip_id = -1;
 
+/* Is SBE timer running? */
+static bool sbe_has_timer = false;
+static bool sbe_timer_in_progress = false;
+
+/* Inflight and next timer in TB */
+static uint64_t sbe_last_gen_stamp;
+static uint64_t sbe_timer_target;
+
+/* Default timeout value */
+#define SBE_TIMER_DEFAULT_MS 10
+static uint64_t sbe_timer_inc_tb;
+
+/* Timer control message */
+static struct sbe_msg *timer_ctrl_msg;
+
+/* Forward declaration */
+static void sbe_timer_start(void);
+
+
 #define SBE_STATUS_PRI_SHIFT	0x30
 #define SBE_STATUS_SEC_SHIFT	0x20
 #define SBE_FFDC_PRESENT	PPC_BIT(1)
@@ -476,6 +496,13 @@  static void sbe_handle_response(struct sbe *sbe)
 	sbe_msg_complete(sbe, msg);
 }
 
+static inline void sbe_check_timer_expiry(void)
+{
+	check_timers(true);
+	sbe_timer_in_progress = false;
+	sbe_timer_start();
+}
+
 void sbe_interrupt(uint32_t chip_id)
 {
 	int rc;
@@ -511,6 +538,8 @@  void sbe_interrupt(uint32_t chip_id)
 			sbe_poke_queue(sbe);
 		}
 		unlock(&sbe->lock);
+
+		sbe_check_timer_expiry();
 		goto clr_interrupt;
 	}
 
@@ -522,6 +551,10 @@  void sbe_interrupt(uint32_t chip_id)
 		unlock(&sbe->lock);
 	}
 
+	/* Timer expired */
+	if (data & SBE_HOST_TIMER_EXPIRY)
+		sbe_check_timer_expiry();
+
 clr_interrupt:
 	rc = xscom_write(chip_id, PSU_HOST_DOORBELL_REG_AND,
 			 SBE_HOST_RESPONSE_CLEAR);
@@ -574,6 +607,121 @@  static void sbe_timeout_poll(void *user_data __unused)
 		sbe_poke_queue(sbe);
 		unlock(&sbe->lock);
 	}
+
+	/*
+	 * Check if the timer is working. If at least SBE_TIMER_DEFAULT_MS
+	 * milliseconds elapsed since last scheduled timer expiry.
+	 */
+	if (sbe_has_timer) {
+		if (tb_compare(now, sbe_last_gen_stamp + sbe_timer_inc_tb)
+		    != TB_AAFTERB)
+			return;
+
+		/*
+		 * In some cases there will be a delay in calling OPAL interrupt
+		 * handler routine (opal_handle_interrupt). In such cases its
+		 * possible that SBE has responded, but OPAL didn't act on that.
+		 * Hence check for SBE response before disabling timer.
+		 */
+		sbe_interrupt(sbe_default_chip_id);
+
+		if (tb_compare(now, sbe_last_gen_stamp + sbe_timer_inc_tb)
+		    != TB_AAFTERB)
+			return;
+
+		prlog(PR_ERR, "Timer stuck, falling back to OPAL pollers.\n");
+		prlog(PR_ERR, "You will likely have slower I2C and may have "
+		      "experienced increased jitter.\n");
+		sbe_has_timer = false;
+	}
+}
+
+static void sbe_timer_start(void)
+{
+	int rc;
+	u32 ms = SBE_TIMER_DEFAULT_MS;
+	u64 tb_cnt, now = mftb();
+
+	if (!sbe_has_timer)
+		return;
+
+	if (sbe_timer_in_progress) {
+		if (sbe_timer_target < now ||
+		    sbe_timer_target > sbe_last_gen_stamp)
+			return;
+
+		/* Update timer control message */
+		timer_ctrl_msg->reg[0] &= 0xffff;
+		timer_ctrl_msg->reg[0] |= ((u64)CONTROL_TIMER_STOP << 32);
+
+		rc = sbe_sync_msg(sbe_default_chip_id, timer_ctrl_msg, false);
+		if (rc != SBE_STATUS_PRI_SUCCESS) {
+			/*
+			 * Lets hope SBE will respond whenever original
+			 * timer expires.
+			 */
+			prlog(PR_ERR, "Failed to stop timer [chip id = %x]\n",
+			      sbe_default_chip_id);
+			return;
+		}
+	}
+
+	if (now < sbe_timer_target &&
+	    sbe_timer_target < now + sbe_timer_inc_tb) {
+		/* Calculate how many ms from now, rounded up */
+		tb_cnt = sbe_timer_target - now + msecs_to_tb(1) - 1;
+		ms = tb_to_msecs(tb_cnt);
+	}
+
+	/* Update timer control message */
+	timer_ctrl_msg->reg[0] &= 0xffff;
+	timer_ctrl_msg->reg[0] |= ((u64)CONTROL_TIMER_START << 32);
+	timer_ctrl_msg->reg[1] = ms;
+
+	rc = sbe_sync_msg(sbe_default_chip_id, timer_ctrl_msg, false);
+	if (rc != SBE_STATUS_PRI_SUCCESS) {
+		prlog(PR_ERR, "Failed to start timer [chip id = %x]\n",
+		      sbe_default_chip_id);
+		return;
+	}
+
+	/* Update last scheduled timer value */
+	sbe_last_gen_stamp = now + msecs_to_tb(ms);
+
+	sbe_timer_in_progress = true;
+}
+
+void sbe_update_timer_expiry(uint64_t new_target)
+{
+	if (!sbe_has_timer || new_target == sbe_timer_target)
+		return;
+
+	sbe_timer_target = new_target;
+	sbe_timer_start();
+}
+
+/* Initialize SBE timer */
+static void sbe_timer_init(void)
+{
+	/* Do not request ack/response for timer message */
+	timer_ctrl_msg = sbe_mkmsg(SBE_CMD_CONTROL_TIMER,
+				   CONTROL_TIMER_START, 0, 0, 0);
+	assert(timer_ctrl_msg);
+
+	sbe_has_timer = true;
+	sbe_timer_target = mftb();
+	sbe_last_gen_stamp = ~0ull;
+	sbe_timer_inc_tb = msecs_to_tb(SBE_TIMER_DEFAULT_MS);
+
+	prlog(PR_INFO, "Timer facility on chip %x, default resolution %dms\n",
+	      sbe_default_chip_id, SBE_TIMER_DEFAULT_MS);
+
+	sbe_timer_start();
+}
+
+bool sbe_timer_ok(void)
+{
+	return sbe_has_timer;
 }
 
 void sbe_init(void)
@@ -609,6 +757,9 @@  void sbe_init(void)
 		return;
 	}
 
+	/* Initiate SBE timer */
+	sbe_timer_init();
+
 	/* Initiate SBE timeout poller */
 	opal_add_poller(sbe_timeout_poll, NULL);
 }
diff --git a/include/sbe.h b/include/sbe.h
index 18609c6..a9eef23 100644
--- a/include/sbe.h
+++ b/include/sbe.h
@@ -228,4 +228,10 @@  extern void sbe_init(void);
 /* SBE interrupt */
 extern void sbe_interrupt(uint32_t chip_id);
 
+/* Is SBE timer available ? */
+extern bool sbe_timer_ok(void);
+
+/* Update SBE timer expiry */
+extern void sbe_update_timer_expiry(uint64_t new_target);
+
 #endif	/* __SBE_H */