void writemailbox(unsigned int channel, unsigned int data)
{
	/* Wait for mailbox to be not full */
	while (*MAILBOX0STATUS & MAILBOX_FULL)
	{
		/* Need to check if this is the right thing to do */
		flushcache();
	}

	dmb();
	*MAILBOX0WRITE = (data | channel);
}

void hy32b_write_reg(unsigned char reg, unsigned short val)
{
    /* put reg address on data bus */
    writeb(reg, FIO0PIN);
    writeb(0, FIO0PIN+2);

    /* RS low, using bit-bang address. */
    writeb(0, P1_BITBANG+4);
    dmb();

    /* Strobe CS_n, using bit-bang address. */
    writeb(0, P1_BITBANG);
    dmb();
    writeb(1, P1_BITBANG);
    dmb();

    writeb(val&0xFF, FIO0PIN);
    writeb((val>>8)&0xFF, FIO0PIN+2);

    /* RS high, using bit-bang address. */
    writeb(1, P1_BITBANG+4);
    dmb();

    /* Strobe CS_n, using bit-bang address. */
    writeb(0, P1_BITBANG);
    dmb();
    writeb(1, P1_BITBANG);
    dmb();

}

/*
 * keyb_notifier
 *
 * Notify keyboard_process, the keyboard was pressed.
 */
void keyb_notifier(PROCESS self, PARAM param) {
  volatile unsigned int cpsr_flag;
  Keyb_Message msg;
  BYTE key;
  //    kprintf("Keyb notifier start\n");
  assert(keyboard_address == 0);

  while (1) {
    SAVE_CPSR_DIS_IRQ(cpsr_flag);
    dmb();
    keyboard_update();
    key = keyboard_get_char();
    //        kprintf("Got new key %x\n", key);
    dmb();
    RESUME_CPSR(cpsr_flag);
    if (key != 0) {
      new_key = key;
      msg.key_buffer = (BYTE *)&new_key;
      message(keyb_port, &msg);
    }
  }
}

int mdss_dsi_v2_phy_init(struct mipi_panel_info *mipi, uint32_t ctl_base)
{
	struct mdss_dsi_phy_ctrl *pd;
	uint32_t i, ln, off = 0, offset;

	pd = mipi->mdss_dsi_phy_db;
	/* DSI PHY configuration */
	off = 0x480;
	writel(pd->strength[0], ctl_base + off + (4 * 0));
	writel(pd->strength[1], ctl_base + off + (4 * 2));

	off = 0x470;
	writel(0x10, ctl_base + off + (4 * 3));
	writel(0x5F, ctl_base + off + (4 * 0));

	off = 0x500;
	/* use LDO mode */
	writel(0x25, ctl_base + 0x4B0);
	for (i = 0; i < 5; i++)
		writel(pd->regulator[i], ctl_base + off + (4 * i));

	mipi_dsi_calibration(ctl_base);

	/* 4 lanes + clk lane configuration */
	/* lane config n * (0 - 4) & DataPath setup */
	for (ln = 0; ln < 5; ln++) {
		off = 0x0300 + (ln * 0x40);
		for (i = 0; i < 9; i++) {
			offset = i + (ln * 9);
			writel(pd->laneCfg[offset], ctl_base + off);
			dmb();
			off += 4;
		}
	}

	off = 0x440;
	for (i = 0; i < 12; i++)
		writel(pd->timing[i], ctl_base + off + (4 * i));

	if (1 == mipi->num_of_lanes)
		writel(0x8, ctl_base + 0x200 + (4 * 11));


	if (mipi->lane_swap)
		writel(mipi->lane_swap, ctl_base + 0x0ac);

	/* T_CLK_POST, T_CLK_PRE for CLK lane P/N HS 200 mV timing
	length should > data lane HS timing length */
	writel(0x41b, ctl_base + 0x0c0);
	return 0;
}

static inline void __b15_rac_flush(void)
{
	u32 reg;

	__raw_writel(FLUSH_RAC, b15_rac_base + rac_flush_offset);
	do {
		/* This dmb() is required to force the Bus Interface Unit
		 * to clean oustanding writes, and forces an idle cycle
		 * to be inserted.
		 */
		dmb();
		reg = __raw_readl(b15_rac_base + rac_flush_offset);
	} while (reg & FLUSH_RAC);
}

/**
* @brief Based on the cpu_id secondary linux start address is defined 
*
* @param cpu_id
*/
void start_secondary_linux(u32 cpu_id)
{
    switch (cpu_id) {
        case 0:
            secondary_start_config_reg[cpu_id] = KERNEL_START_ADDR;
            break;
        case 1:
            secondary_start_config_reg[cpu_id] = KERNEL_START_ADDR;
            break;
        default:
            break;
    }
    dmb();
}

void mdss_dsi_phy_contention_detection(
		struct mipi_panel_info *mipi,
		uint32_t phy_base)
{
        struct mdss_dsi_phy_ctrl *pd;

	if ((mipi->mdss_dsi_phy_db->pll_type == DSI_PLL_TYPE_THULIUM) ||
		(mdp_get_revision() == MDP_REV_304))
                return;

        pd = (mipi->mdss_dsi_phy_db);
	writel(pd->strength[1], phy_base + MMSS_DSI_PHY_STRENGTH_CTRL_1);
	dmb();
}

static void __init mem_init()
{
	unsigned int i;

	/* copy interrupt vector table to 0x0 */
	for (i = 0; i < NR_VECTOR * WORD_SIZE * 2; i += WORD_SIZE)
		*(unsigned int *)(0 + i) = *(unsigned int *)(0x8000 + i);

	/* clear .bss section */
	extern char _bss, _ebss;
	for (i = 0; (&_bss + i) < &_ebss; i++)
		*((char *)&_bss + i) = 0;

	dmb();
}

void meson_cpu_die(unsigned int cpu)
{
	meson_set_cpu_ctrl_reg(cpu, 0);
	flush_cache_all();
	dsb();
	dmb();

	meson_cleanup();
	aml_set_reg32_bits(MESON_CPU_POWER_CTRL_REG,0x3,(cpu << 3),2);
	asm volatile(
		"dsb\n"
		"wfi\n"
	);
	BUG();
}

static void msm_restart(char str, const char *cmd)
{
	printk(KERN_NOTICE "Going down for restart now\n");
	writel(1, WDT0_RST);
	writel(0, WDT0_EN);
	writel(0x31F3, WDT0_BARK_TIME);
	writel(3, WDT0_EN);
	dmb();
	if (tcsr_base != NULL)
		writel(3, tcsr_base + TCSR_WDT_CFG);

	mdelay(10000);
	printk(KERN_ERR "Restarting has failed\n");
	return;
}

void reboot_device(uint32_t reboot_reason)
{

	/* TBD - set download mode? */
	
	pm8058_reset_pwr_off(1);

	writel(reboot_reason, RESTART_REASON_ADDR);
	dmb();

	writel(0, MSM_WDT0_EN);
	writel(0, PSHOLD_CTL_SU);
	mdelay(5000);

	writel(0x31F3, MSM_WDT0_BARK_TIME);
	writel(0x31F3, MSM_WDT0_BITE_TIME);
	writel(3, MSM_WDT0_EN);
	dmb();

	secure_writel(3, MSM_TCSR_WDOG_CFG);
	mdelay(10000);

	dprintf(CRITICAL, "Shutdown failed\n");
}

static int __cpuinit scorpion_release_secondary(void)
{
	void *base_ptr = ioremap_nocache(0x00902000, SZ_4K*2);
	if (!base_ptr)
		return -EINVAL;

	writel_relaxed(0x0, base_ptr+0x15A0);
	dmb();
	writel_relaxed(0x0, base_ptr+0xD80);
	writel_relaxed(0x3, base_ptr+0xE64);
	mb();
	iounmap(base_ptr);

	return 0;
}

static int scorpion_release_secondary(void)
{
	void *base_ptr = ioremap_nocache(0x00902000, SZ_4K*2);
	if (!base_ptr)
		return -EINVAL;

	writel_relaxed(0, base_ptr + VDD_SC1_ARRAY_CLAMP_GFS_CTL);
	dmb();
	writel_relaxed(0, base_ptr + SCSS_CPU1CORE_RESET);
	writel_relaxed(3, base_ptr + SCSS_DBG_STATUS_CORE_PWRDUP);
	mb();
	iounmap(base_ptr);

	return 0;
}

int
process_sent_bds(XEmacPs_BdRing *txring)
{
	XEmacPs_Bd *txbdset;
	XEmacPs_Bd *CurBdPntr;
	int n_bds;
	XStatus Status;
	int n_pbufs_freed = 0;
	unsigned int BdIndex;
	struct pbuf *p;
	unsigned int *Temp;

	/* obtain processed BD's */
	n_bds = XEmacPs_BdRingFromHwTx(txring, XLWIP_CONFIG_N_TX_DESC, &txbdset);
	if (n_bds == 0)  {
		return;
	}

	/* free the processed BD's */
	n_pbufs_freed = n_bds;
	CurBdPntr = txbdset;
	while (n_pbufs_freed > 0) {
		BdIndex = XEMACPS_BD_TO_INDEX(txring, CurBdPntr);
		Temp = (unsigned int *)CurBdPntr;
		*Temp = 0;
		Temp++;
		*Temp = 0x80000000;
		if (BdIndex == (XLWIP_CONFIG_N_TX_DESC - 1)) {
			*Temp = 0xC0000000;
		}

		p = (struct pbuf *)tx_pbufs_storage[BdIndex];
		if(p != NULL) {
			pbuf_free(p);
		}
		tx_pbufs_storage[BdIndex] = 0;
		CurBdPntr = XEmacPs_BdRingNext(txring, CurBdPntr);
		n_pbufs_freed--;
		dmb();
		dsb();
	}

	Status = XEmacPs_BdRingFree(txring, n_bds, txbdset);
	if (Status != XST_SUCCESS) {
		LWIP_DEBUGF(NETIF_DEBUG, ("Failure while freeing in Tx Done ISR\r\n"));
	}
	return 0;
}

void reboot_device(unsigned reboot_reason)
{
	/* Reset WDG0 counter */
	writel(1, MSM_WDT0_RST);
	/* Disable WDG0 */
	writel(0, MSM_WDT0_EN);
	/* Set WDG0 bark time */
	writel(0x31F3, MSM_WDT0_BT);
	/* Enable WDG0 */
	writel(3, MSM_WDT0_EN);
	dmb();
	/* Enable WDG output */
	secure_writel(3, MSM_TCSR_BASE + TCSR_WDOG_CFG);
	mdelay(10000);
	dprintf(CRITICAL, "Rebooting failed\n");
	return;
}

static int wait_for_mc(bool booting_mc, u32 *final_reg_gsr)
{
	u32 reg_gsr;
	u32 mc_fw_boot_status;
	unsigned long timeout_ms = get_mc_boot_timeout_ms();
	struct mc_ccsr_registers __iomem *mc_ccsr_regs = MC_CCSR_BASE_ADDR;

	dmb();
	assert(timeout_ms > 0);
	for (;;) {
		udelay(1000);	/* throttle polling */
		reg_gsr = in_le32(&mc_ccsr_regs->reg_gsr);
		mc_fw_boot_status = (reg_gsr & GSR_FS_MASK);
		if (mc_fw_boot_status & 0x1)
			break;

		timeout_ms--;
		if (timeout_ms == 0)
			break;
	}

	if (timeout_ms == 0) {
		printf("ERROR: timeout\n");

		/* TODO: Get an error status from an MC CCSR register */
		return -ETIMEDOUT;
	}

	if (mc_fw_boot_status != 0x1) {
		/*
		 * TODO: Identify critical errors from the GSR register's FS
		 * field and for those errors, set error to -ENODEV or other
		 * appropriate errno, so that the status property is set to
		 * failure in the fsl,dprc device tree node.
		 */
		printf("WARNING: Firmware returned an error (GSR: %#x)\n",
		       reg_gsr);
	} else {
		printf("SUCCESS\n");
	}


	*final_reg_gsr = reg_gsr;
	return 0;
}

int clk_suspend_usb(struct platform_device *pdev, const char *s_clock_name,
		unsigned long usb_peri_reg,
		const char *cpu_type)
{
	if (!pdev)
		return -1;

	if (!strcmp(cpu_type, MESON8))
			clk_disable_usb_meson8(pdev,
				s_clock_name, usb_peri_reg);

	if (!strcmp(cpu_type, GXBABY))
			clk_disable_usb_gxbaby(pdev,
				s_clock_name, usb_peri_reg);

	dmb(4);
	return 0;
}

void 
PFEMElement2DBubble::getdL(const Vector& p, Matrix& dl) const {
    Matrix gb(2,3);
    getGbub(gb);

    double invmb = getinvMbub();

    getdGbt(gb*p*invmb, dl);

    Matrix dmb(2,6);
    getdinvMbub(gb*p, dmb);

    dl.addMatrixTransposeProduct(1.0,gb,dmb,1.0);
            
    Matrix dgb(2,6);
    getdGb(p,dgb);
    dl.addMatrixTransposeProduct(1.0,gb,dgb,invmb);
}

static int measure_clk_set_parent(struct clk *c, struct clk *parent)
{
	int ret = 0;
	uint32_t clk_sel;
	struct measure_sel *p;
	struct measure_clk *clk = to_measure_clk(c);

	if (!parent)
		return ERR_INVALID_ARGS;

	p = find_measure_sel(parent);
	if (!p)
		return ERR_INVALID_ARGS;

	/*
	 * Program the test vector, measurement period (sample_ticks)
	 * and scaling multiplier.
	 */
	clk->sample_ticks = 0x10000;
	clk_sel = p->test_vector & TEST_CLK_SEL_MASK;
	clk->multiplier = 1;
	switch (p->test_vector >> TEST_TYPE_SHIFT) {
	case TEST_TYPE_PER_LS:
		writel_relaxed(0x4030D00|BVAL(7, 0, clk_sel), CLK_TEST_REG);
		break;
	case TEST_TYPE_PER_HS:
		writel_relaxed(0x4020000|BVAL(16, 10, clk_sel), CLK_TEST_REG);
		break;
	case TEST_TYPE_MM_LS:
		writel_relaxed(0x4030D97, CLK_TEST_REG);
		writel_relaxed(BVAL(6, 1, clk_sel)|BIT(0), DBG_CFG_REG_LS_REG);
		break;
	case TEST_TYPE_MM_HS:
		writel_relaxed(0x402B800, CLK_TEST_REG);
		writel_relaxed(BVAL(6, 1, clk_sel)|BIT(0), DBG_CFG_REG_HS_REG);
		break;
	default:
		ret = ERR_NOT_SUPPORTED;
	}
	/* Make sure test vector is set before starting measurements. */
	dmb();

	return ret;
}

void platform_test_loop_ticks()
{
	/* Initialize the timer */
	unsigned long timer_base =
		PLATFORM_TIMER0_VBASE + SP804_TIMER1_OFFSET;
	volatile u32 reg = read(timer_base + SP804_CTRL);

        const int looptotal = 500000;
	int ticks, loops = looptotal;
	int inst_per_loop = 2;
	const int timer_load = 0xFFFFFFFF;
	int timer_read;
	int ipm_whole, ipm_decimal, temp;

	/* Make sure timer is disabled */
	write(0, timer_base + SP804_CTRL);

	/* Load the timer with a full value */
	write(timer_load, timer_base + SP804_LOAD);

	/* One shot, 32 bits, no irqs */
	reg = SP804_32BIT | SP804_ONESHOT | SP804_ENABLE;

	/* Start the timer */
	write(reg, timer_base + SP804_CTRL);
	dmb(); /* Make sure write occurs before looping */

	busy_loop(loops);

	timer_read = read(timer_base + SP804_VALUE);

	ticks = timer_load - timer_read;

	temp = (inst_per_loop * looptotal) * 10 / ticks;
	ipm_whole = temp / 10;
	ipm_decimal = temp - ipm_whole * 10;

	printk("Perfmon: %d ticks/%d instructions\n",
	       ticks, inst_per_loop * looptotal);

	printk("Perfmon: %d%d instr/Mhz.\n",
	       ipm_whole, ipm_decimal);
}