void epd_enable(void)
{
	char temp = TEMP_USE_DEFAULT;
	debug("epd_enable\n");
	
	//epdc_power_on();
	/* Draw data to display */
	//draw_mode0();

	/* Enable clock gating (clear to enable) */
	REG_CLR(EPDC_BASE, EPDC_CTRL, EPDC_CTRL_CLKGATE);
	while (REG_RD(EPDC_BASE, EPDC_CTRL) &
	       (EPDC_CTRL_SFTRST | EPDC_CTRL_CLKGATE))
		;

	REG_WR(EPDC_BASE, EPDC_TEMP, TEMP_USE_DEFAULT);
	temp = read_temperature();
//	temp = do_read_temperature_via_i2c();
	temp_set_index(temp);
	
	/* Set Waveform Bufferr register to real waveform address */
	REG_WR(EPDC_BASE, EPDC_WVADDR, panel_info.epdc_data.waveform_buf_addr);

	debug("epdc_irq's value %08x\nEPDC_IRQ_CLR's value %08x\n",REG_RD(EPDC_BASE, EPDC_IRQ), REG_RD(EPDC_BASE, EPDC_IRQ_CLR));
	debug("EPDC LUT STATUS %08x\n",REG_RD(EPDC_BASE,EPDC_STATUS_LUTS));
	draw_splash_screen();
	debug("epdc_irq's value %08x\nEPDC_IRQ_CLR's value %08x\n",REG_RD(EPDC_BASE, EPDC_IRQ), REG_RD(EPDC_BASE, EPDC_IRQ_CLR));
	debug("EPDC LUT STATUS %08x\n",REG_RD(EPDC_BASE,EPDC_STATUS_LUTS));
}

static void
console_write_direct(struct console *co, const char *buf, unsigned int len)
{
	int i;
	reg_ser_r_stat_din stat;
	reg_ser_rw_tr_dma_en tr_dma_en, old;

	/* Switch to manual mode */
	tr_dma_en = old = REG_RD (ser, port->instance, rw_tr_dma_en);
	if (tr_dma_en.en == regk_ser_yes) {
		tr_dma_en.en = regk_ser_no;
		REG_WR(ser, port->instance, rw_tr_dma_en, tr_dma_en);
	}

	/* Send data */
	for (i = 0; i < len; i++) {
		/* LF -> CRLF */
		if (buf[i] == '\n') {
			do {
				stat = REG_RD (ser, port->instance, r_stat_din);
			} while (!stat.tr_rdy);
			REG_WR_INT (ser, port->instance, rw_dout, '\r');
		}
		/* Wait until transmitter is ready and send.*/
		do {
			stat = REG_RD (ser, port->instance, r_stat_din);
		} while (!stat.tr_rdy);
		REG_WR_INT (ser, port->instance, rw_dout, buf[i]);
	}

	/* Restore mode */
	if (tr_dma_en.en != old.en)
		REG_WR(ser, port->instance, rw_tr_dma_en, old);
}

int32_t    scale_continue(void)
{
	enum scale_drv_rtn      rtn = SCALE_RTN_SUCCESS;
	uint32_t                slice_h = g_path->slice_height;

	SCALE_TRACE("SCALE DRV: continue %d, %d, %d \n",
		g_path->slice_height, g_path->slice_in_height, g_path->scale_mode);

	if (SCALE_MODE_SLICE == g_path->scale_mode) {
		if (g_path->slice_in_height +  g_path->slice_height >= g_path->input_rect.h) {
			slice_h = g_path->input_rect.h - g_path->slice_in_height;
			g_path->is_last_slice = 1;
			REG_MWR(SCALE_SLICE_VER, 0x3FF, slice_h);
			REG_OWR(SCALE_SLICE_VER, (1 << 12));
			SCALE_TRACE("SCALE DRV: continue, last slice, 0x%x \n", REG_RD(SCALE_SLICE_VER));
		} else {
			g_path->is_last_slice = 0;
			REG_MWR(SCALE_SLICE_VER, (1 << 12), (0 << 12));
		}
		g_path->slice_in_height += g_path->slice_height;
	}

	REG_WR(SCALE_FRM_SWAP_Y, g_path->temp_buf_addr.yaddr);
	REG_WR(SCALE_FRM_SWAP_U, g_path->temp_buf_addr.uaddr);
	REG_WR(SCALE_FRM_LINE,   g_path->temp_buf_addr.vaddr);
	
	_scale_reg_trace();
	REG_OWR(SCALE_CFG, 1);
	atomic_inc(&g_path->start_flag);
	SCALE_TRACE("SCALE DRV: continue %x.\n", REG_RD(SCALE_CFG));

	return rtn;
}

static int32_t isp_k_pre_cdn_rgb_block(struct isp_io_param *param)
{
	int32_t ret = 0;
	uint32_t val = 0;
	struct isp_dev_pre_cdn_rgb_info pcr_info;

	memset(&pcr_info, 0x00, sizeof(pcr_info));

	ret = copy_from_user((void *)&pcr_info, param->property_param, sizeof(pcr_info));
	if (0 != ret) {
		printk("isp_k_pre_cdn_rgb_block: copy error, ret=0x%x\n", (uint32_t)ret);
		return -1;
	}

	REG_MWR(ISP_PRECNRNEW_CFG, BIT_0, pcr_info.bypass);

	REG_MWR(ISP_PRECNRNEW_CFG, 0x3 << 1, pcr_info.median_mode << 1);

	val = pcr_info.median_thr & 0xFFFF;
	REG_WR(ISP_PRECNRNEW_MEDIAN_THR, val);
	val = (pcr_info.thru0 & 0xFFFF) | ((pcr_info.thru1 & 0xFFFF) << 16);
	REG_WR(ISP_PRECNRNEW_THRU, val);
	val = (pcr_info.thrv0 & 0xFFFF) | ((pcr_info.thrv1 & 0xFFFF) << 16);
	REG_WR(ISP_PRECNRNEW_THRV, val);

	return ret;

}

static void epdc_submit_update(u32 lut_num, u32 waveform_mode, u32 update_mode,
			       int use_cp_val, u32 cp_val, int use_np_val, u32 np_val)
{
	u32 reg_val = 0;
	u32 fix_val = 0;

	if (use_cp_val) {
		fix_val |=
			((np_val << EPDC_UPD_FIXED_FIXCP_OFFSET) &
			EPDC_UPD_FIXED_FIXCP_MASK) | EPDC_UPD_FIXED_FIXCP_EN;

		reg_val = EPDC_UPD_CTRL_USE_FIXED;
	}
	if (use_np_val) {
		fix_val |=
			((np_val << EPDC_UPD_FIXED_FIXNP_OFFSET) &
			EPDC_UPD_FIXED_FIXNP_MASK) | EPDC_UPD_FIXED_FIXNP_EN;

		reg_val = EPDC_UPD_CTRL_USE_FIXED;
	}
	
	REG_WR(EPDC_BASE, EPDC_UPD_FIXED, fix_val);

	reg_val |=
		((lut_num << EPDC_UPD_CTRL_LUT_SEL_OFFSET) &
		EPDC_UPD_CTRL_LUT_SEL_MASK) |
		((waveform_mode << EPDC_UPD_CTRL_WAVEFORM_MODE_OFFSET) &
		EPDC_UPD_CTRL_WAVEFORM_MODE_MASK) |
		update_mode;

	REG_WR(EPDC_BASE, EPDC_UPD_CTRL, reg_val);
}

int epdc_ctrl_init(void *lcdbase)
{
	/*
	 * We rely on lcdbase being a physical address, i.e., either MMU off,
	 * or 1-to-1 mapping. Might want to add some virt2phys here.
	 */
	if (!lcdbase)
		return -1;

	eink_color_fg = 0xFF;
	eink_color_bg = 0xFF;

	/* Reset */
	REG_SET(EPDC_BASE, EPDC_CTRL, EPDC_CTRL_SFTRST);
	while (!(REG_RD(EPDC_BASE, EPDC_CTRL) & EPDC_CTRL_CLKGATE))
		;
	REG_CLR(EPDC_BASE, EPDC_CTRL, EPDC_CTRL_SFTRST);

	/* Enable clock gating (clear to enable) */
	REG_CLR(EPDC_BASE, EPDC_CTRL, EPDC_CTRL_CLKGATE);
	while (REG_RD(EPDC_BASE, EPDC_CTRL) &
	       (EPDC_CTRL_SFTRST | EPDC_CTRL_CLKGATE))
		;

	debug("resolution %dx%d, bpp %d\n", (int)panel_info.vl_col,
		(int)panel_info.vl_row, NBITS(panel_info.vl_bpix));


	/* Set framebuffer pointer */
	REG_WR(EPDC_BASE, EPDC_UPD_ADDR, (u32)lcdbase);

#if 0
	/* Set Working Buffer pointer */
	REG_WR(EPDC_BASE, EPDC_WB_ADDR, panel_info.epdc_data.working_buf_addr);
	/* Set Waveform Buffer pointer */
	REG_WR(EPDC_BASE, EPDC_WVADDR, panel_info.epdc_data.waveform_buf_addr);
#endif 

	/* Set waveform and working buffer, they will be changed later */
	REG_WR(EPDC_BASE, EPDC_WVADDR, (unsigned long)CONFIG_TEMP_INIT_WAVEFORM_ADDR);
	REG_WR(EPDC_BASE, EPDC_WB_ADDR, (unsigned long)CONFIG_WORKING_BUF_ADDR);
	
#if 0
	/* Get waveform data address and offset */
	int data_offs = setup_waveform_file();
	if(data_offs == -1) {
		printf("Can't load waveform data!\n");
		return -1;
	}
#endif

	/* Initialize EPDC, passing pointer to EPDC registers */
	epdc_init_settings();

	epdc_initialized = 1;

	return;
}

static void
start_port(struct dbg_port* p)
{
	if (!p)
		return;

	if (p->started)
		return;
	p->started = 1;

	if (p->nbr == 1)
		crisv32_pinmux_alloc_fixed(pinmux_ser1);
	else if (p->nbr == 2)
		crisv32_pinmux_alloc_fixed(pinmux_ser2);
	else if (p->nbr == 3)
		crisv32_pinmux_alloc_fixed(pinmux_ser3);

	/* Set up serial port registers */
	reg_ser_rw_tr_ctrl tr_ctrl = {0};
	reg_ser_rw_tr_dma_en tr_dma_en = {0};

	reg_ser_rw_rec_ctrl rec_ctrl = {0};
	reg_ser_rw_tr_baud_div tr_baud_div = {0};
	reg_ser_rw_rec_baud_div rec_baud_div = {0};

	tr_ctrl.base_freq = rec_ctrl.base_freq = regk_ser_f29_493;
	tr_dma_en.en = rec_ctrl.dma_mode = regk_ser_no;
	tr_baud_div.div = rec_baud_div.div = 29493000 / p->baudrate / 8;
	tr_ctrl.en = rec_ctrl.en = 1;

	if (p->parity == 'O')
	{
		tr_ctrl.par_en = regk_ser_yes;
		tr_ctrl.par = regk_ser_odd;
		rec_ctrl.par_en = regk_ser_yes;
		rec_ctrl.par = regk_ser_odd;
	}
	else if (p->parity == 'E')
	{
		tr_ctrl.par_en = regk_ser_yes;
		tr_ctrl.par = regk_ser_even;
		rec_ctrl.par_en = regk_ser_yes;
		rec_ctrl.par = regk_ser_odd;
	}

	if (p->bits == 7)
	{
		tr_ctrl.data_bits = regk_ser_bits7;
		rec_ctrl.data_bits = regk_ser_bits7;
	}

	REG_WR (ser, p->instance, rw_tr_baud_div, tr_baud_div);
	REG_WR (ser, p->instance, rw_rec_baud_div, rec_baud_div);
	REG_WR (ser, p->instance, rw_tr_dma_en, tr_dma_en);
	REG_WR (ser, p->instance, rw_tr_ctrl, tr_ctrl);
	REG_WR (ser, p->instance, rw_rec_ctrl, rec_ctrl);
}

static irqreturn_t crisv32_arbiter_irq(int irq, void *dev_id)
{
	reg_marb_r_masked_intr masked_intr =
	    REG_RD(marb, regi_marb, r_masked_intr);
	reg_marb_bp_r_brk_clients r_clients;
	reg_marb_bp_r_brk_addr r_addr;
	reg_marb_bp_r_brk_op r_op;
	reg_marb_bp_r_brk_first_client r_first;
	reg_marb_bp_r_brk_size r_size;
	reg_marb_bp_rw_ack ack = { 0 };
	reg_marb_rw_ack_intr ack_intr = {
		.bp0 = 1, .bp1 = 1, .bp2 = 1, .bp3 = 1
	};
	struct crisv32_watch_entry *watch;

	if (masked_intr.bp0) {
		watch = &watches[0];
		ack_intr.bp0 = regk_marb_yes;
	} else if (masked_intr.bp1) {
		watch = &watches[1];
		ack_intr.bp1 = regk_marb_yes;
	} else if (masked_intr.bp2) {
		watch = &watches[2];
		ack_intr.bp2 = regk_marb_yes;
	} else if (masked_intr.bp3) {
		watch = &watches[3];
		ack_intr.bp3 = regk_marb_yes;
	} else {
		return IRQ_NONE;
	}

	/* Retrieve all useful information and print it. */
	r_clients = REG_RD(marb_bp, watch->instance, r_brk_clients);
	r_addr = REG_RD(marb_bp, watch->instance, r_brk_addr);
	r_op = REG_RD(marb_bp, watch->instance, r_brk_op);
	r_first = REG_RD(marb_bp, watch->instance, r_brk_first_client);
	r_size = REG_RD(marb_bp, watch->instance, r_brk_size);

	printk(KERN_INFO "Arbiter IRQ\n");
	printk(KERN_INFO "Clients %X addr %X op %X first %X size %X\n",
	       REG_TYPE_CONV(int, reg_marb_bp_r_brk_clients, r_clients),
	       REG_TYPE_CONV(int, reg_marb_bp_r_brk_addr, r_addr),
	       REG_TYPE_CONV(int, reg_marb_bp_r_brk_op, r_op),
	       REG_TYPE_CONV(int, reg_marb_bp_r_brk_first_client, r_first),
	       REG_TYPE_CONV(int, reg_marb_bp_r_brk_size, r_size));

	REG_WR(marb_bp, watch->instance, rw_ack, ack);
	REG_WR(marb, regi_marb, rw_ack_intr, ack_intr);

	printk(KERN_INFO "IRQ occured at %lX\n", get_irq_regs()->erp);

	if (watch->cb)
		watch->cb();

	return IRQ_HANDLED;
}

void viewport_draw_image (Viewport_t handle,
                          G2D_Image * image,
                          UINT16 dst_x,
                          UINT16 dst_y,
                          UINT16 dst_w,
                          UINT16 dst_h)
{
    OS_Return res;

    ASSERT(image);
    do
    {
        // Validate the input handle
        if(!g2d_viewport_valid(handle)) break;

        // The width and height should not be 0
        if(!dst_w || !dst_h) break;

        // The width and height of the image should not be 0
        if(!image->width || !image->height) break;

        // Get the viewport object
        G2D_Viewport * vp = &viewports[handle];

        // Wait for the previous command to finish
        while(g2d_isbusy());

        // Activate viewport
        res = g2d_activate_viewport(vp);
        if(res != SUCCESS) break;

        // Set Destination Left Top and Right Bottom Coordinate Registers
        g2d_set_dest_coordinates(vp, dst_x, dst_y, dst_w, dst_h);

        // Check if we need to stretch the image
        BOOL stretch = ((image->width != dst_w) || (image->height != dst_h));

        // Src buffer clear (Automatically set to 0b after a cycle)
        REG_WR(CACHECTL_REG, G2D_FLUSH_SRC_BUFFER);

        // Source properties
        REG_WR(SRC_SELECT_REG, G2D_SELECT_MODE_NORMAL);					// Source Image Selection Register
        REG_WR(SRC_COLOR_MODE_REG, image->format | G2D_ORDER_AXRGB);	// Source Image Color Mode Register
        REG_WR(SRC_STRIDE_REG, (image->width * gColorDepthMap[image->format]) >> 3);	// Set Source Stride Register
        REG_WR(SRC_BASE_ADDR_REG, (UINT32)image->buffer);
        REG_WR(SRC_LEFT_TOP_REG, 0);
        REG_WR(SRC_RIGHT_BOTTOM_REG, image->width | (image->height << 16));

        // Set ROP4 register
        REG_WR(ROP4_REG, ROP4_COPY);

        // Start the BitBLT Command Register
        REG_WR(BITBLT_COMMAND_REG, G2D_ENABLE_CLIPPING_WINDOW | (stretch ? G2D_ENABLE_STRETCH_MODE : 0));
        REG_WR(BITBLT_START_REG, 1);

    } while(0);
}

/* Maps the specified queue to the specified COS */
void ecore_map_q_cos(struct _lm_device_t *pdev, u32_t q_num, u32_t new_cos)
{
	/* find current COS mapping */
	u32_t curr_cos = REG_RD(pdev, QM_REG_QVOQIDX_0 + q_num * 4);

	/* check if queue->COS mapping has changed */
	if (curr_cos != new_cos) {
		u32_t num_vnics = ECORE_PORT2_MODE_NUM_VNICS;
		u32_t reg_addr, reg_bit_map, vnic;

		/* update parameters for 4port mode */
		if (INIT_MODE_FLAGS(pdev) & MODE_PORT4) {
			num_vnics = ECORE_PORT4_MODE_NUM_VNICS;
			if (PORT_ID(pdev)) {
				curr_cos += ECORE_E3B0_PORT1_COS_OFFSET;
				new_cos += ECORE_E3B0_PORT1_COS_OFFSET;
			}
		}

		/* change queue mapping for each VNIC */
		for (vnic = 0; vnic < num_vnics; vnic++) {
			u32_t pf_q_num =
				ECORE_PF_Q_NUM(q_num, PORT_ID(pdev), vnic);
			u32_t q_bit_map = 1 << (pf_q_num & 0x1f);

			/* overwrite queue->VOQ mapping */
			REG_WR(pdev, ECORE_Q_VOQ_REG_ADDR(pf_q_num), new_cos);

			/* clear queue bit from current COS bit map */
			reg_addr = ECORE_VOQ_Q_REG_ADDR(curr_cos, pf_q_num);
			reg_bit_map = REG_RD(pdev, reg_addr);
			REG_WR(pdev, reg_addr, reg_bit_map & (~q_bit_map));

			/* set queue bit in new COS bit map */
			reg_addr = ECORE_VOQ_Q_REG_ADDR(new_cos, pf_q_num);
			reg_bit_map = REG_RD(pdev, reg_addr);
			REG_WR(pdev, reg_addr, reg_bit_map | q_bit_map);

			/* set/clear queue bit in command-queue bit map
			(E2/E3A0 only, valid COS values are 0/1) */
			if (!(INIT_MODE_FLAGS(pdev) & MODE_E3_B0)) {
				reg_addr = ECORE_Q_CMDQ_REG_ADDR(pf_q_num);
				reg_bit_map = REG_RD(pdev, reg_addr);
				q_bit_map = 1 << (2 * (pf_q_num & 0xf));
				reg_bit_map = new_cos ?
					      (reg_bit_map | q_bit_map) :
					      (reg_bit_map & (~q_bit_map));
				REG_WR(pdev, reg_addr, reg_bit_map);
			}
		}
	}
}

void
MM_AnTxConfig(
    PAN_STATE_INFO pAnInfo)
{
    PLM_DEVICE_BLOCK pDevice;

    pDevice = (PLM_DEVICE_BLOCK) pAnInfo->pContext;

    REG_WR(pDevice, MacCtrl.TxAutoNeg, (LM_UINT32) pAnInfo->TxConfig.AsUSHORT);

    pDevice->MacMode |= MAC_MODE_SEND_CONFIGS;
    REG_WR(pDevice, MacCtrl.Mode, pDevice->MacMode);
}

static inline void etraxfs_uart_start_tx_bottom(struct uart_port *port)
{
	struct uart_cris_port *up = (struct uart_cris_port *)port;
	void __iomem *regi_ser = up->regi_ser;
	reg_ser_rw_tr_ctrl tr_ctrl;
	reg_ser_rw_intr_mask intr_mask;

	tr_ctrl = REG_RD(ser, regi_ser, rw_tr_ctrl);
	tr_ctrl.stop = regk_ser_no;
	REG_WR(ser, regi_ser, rw_tr_ctrl, tr_ctrl);
	intr_mask = REG_RD(ser, regi_ser, rw_intr_mask);
	intr_mask.tr_rdy = regk_ser_yes;
	REG_WR(ser, regi_ser, rw_intr_mask, intr_mask);
}

static void sync_serial_start_port(struct sync_port *port)
{
	reg_sser_rw_cfg cfg = REG_RD(sser, port->regi_sser, rw_cfg);
	reg_sser_rw_tr_cfg tr_cfg =
		REG_RD(sser, port->regi_sser, rw_tr_cfg);
	reg_sser_rw_rec_cfg rec_cfg =
		REG_RD(sser, port->regi_sser, rw_rec_cfg);
	cfg.en = regk_sser_yes;
	tr_cfg.tr_en = regk_sser_yes;
	rec_cfg.rec_en = regk_sser_yes;
	REG_WR(sser, port->regi_sser, rw_cfg, cfg);
	REG_WR(sser, port->regi_sser, rw_tr_cfg, tr_cfg);
	REG_WR(sser, port->regi_sser, rw_rec_cfg, rec_cfg);
	port->started = 1;
}

void lcd_ctrl_init(void *lcdbase)
{
	/*
	 * We rely on lcdbase being a physical address, i.e., either MMU off,
	 * or 1-to-1 mapping. Might want to add some virt2phys here.
	 */
	if (!lcdbase)
		return;

	lcd_color_fg = 0xFF;
	lcd_color_bg = 0xFF;

	/* Reset */
	REG_SET(EPDC_BASE, EPDC_CTRL, EPDC_CTRL_SFTRST);
	while (!(REG_RD(EPDC_BASE, EPDC_CTRL) & EPDC_CTRL_CLKGATE))
		;
	REG_CLR(EPDC_BASE, EPDC_CTRL, EPDC_CTRL_SFTRST);

	/* Enable clock gating (clear to enable) */
	REG_CLR(EPDC_BASE, EPDC_CTRL, EPDC_CTRL_CLKGATE);
	while (REG_RD(EPDC_BASE, EPDC_CTRL) &
	       (EPDC_CTRL_SFTRST | EPDC_CTRL_CLKGATE))
		;

	debug("resolution %dx%d, bpp %d\n", (int)panel_info.vl_col,
		(int)panel_info.vl_row, NBITS(panel_info.vl_bpix));

	/* Set framebuffer pointer */
	REG_WR(EPDC_BASE, EPDC_UPD_ADDR, (u32)lcdbase);

	/* Set Working Buffer pointer */
	REG_WR(EPDC_BASE, EPDC_WB_ADDR, panel_info.epdc_data.working_buf_addr);

	/* Get waveform data address and offset */
	if (setup_waveform_file()) {
		printf("Can't load waveform data!\n");
		return;
	}

	/* Set Waveform Buffer pointer */
	REG_WR(EPDC_BASE, EPDC_WVADDR,
		panel_info.epdc_data.waveform_buf_addr);

	/* Initialize EPDC, passing pointer to EPDC registers */
	epdc_init_settings();

	return;
}

static int32_t isp_k_prefilter_block(struct isp_io_param *param)
{
	int32_t ret = 0;
	uint32_t val = 0;
	struct isp_dev_prefilter_info prefilter_info;

	memset(&prefilter_info, 0x00, sizeof(prefilter_info));

	ret = copy_from_user((void *)&prefilter_info, param->property_param, sizeof(prefilter_info));
	if (0 != ret) {
		printk("isp_k_prefilter_block: copy error, ret=0x%x\n", (uint32_t)ret);
		return -1;
	}

	val= (prefilter_info.writeback & 0x03) << 1;
	REG_MWR(ISP_PREF_PARAM, (BIT_2 | BIT_1), val);

	val = ((prefilter_info.v_thrd & 0xFF) << 16)
		| ((prefilter_info.u_thrd & 0xFF) << 8)
		| (prefilter_info.y_thrd & 0xFF);
	REG_WR(ISP_PREF_THRD, (val & 0xFFFFFF));

	if (prefilter_info.bypass) {
		REG_OWR(ISP_PREF_PARAM, BIT_0);
	} else {
		REG_MWR(ISP_PREF_PARAM, BIT_0, 0);
	}

	return ret;
}

void
hard_reset_now(void)
{
	/*
	 * Don't declare this variable elsewhere.  We don't want any other
	 * code to know about it than the watchdog handler in entry.S and
	 * this code, implementing hard reset through the watchdog.
	 */
#if defined(CONFIG_ETRAX_WATCHDOG)
	extern int cause_of_death;
#endif

	printk("*** HARD RESET ***\n");
	local_irq_disable();

#if defined(CONFIG_ETRAX_WATCHDOG)
	cause_of_death = 0xbedead;
#else
{
	reg_timer_rw_wd_ctrl wd_ctrl = {0};

	stop_watchdog();

	wd_ctrl.key = 16;	/* Arbitrary key. */
	wd_ctrl.cnt = 1;	/* Minimum time. */
	wd_ctrl.cmd = regk_timer_start;

        arch_enable_nmi();
	REG_WR(timer, regi_timer0, rw_wd_ctrl, wd_ctrl);
}
#endif

	while (1)
		; /* Wait for reset. */
}

/* Secondary CPUs starts using C here. Here we need to setup CPU
 * specific stuff such as the local timer and the MMU. */
void __init smp_callin(void)
{
	extern void cpu_idle(void);

	int cpu = cpu_now_booting;
	reg_intr_vect_rw_mask vect_mask = {0};

	/* Initialise the idle task for this CPU */
	atomic_inc(&init_mm.mm_count);
	current->active_mm = &init_mm;

	/* Set up MMU */
	cris_mmu_init();
	__flush_tlb_all();

	/* Setup local timer. */
	cris_timer_init();

	/* Enable IRQ and idle */
	REG_WR(intr_vect, irq_regs[cpu], rw_mask, vect_mask);
	unmask_irq(IPI_INTR_VECT);
	unmask_irq(TIMER0_INTR_VECT);
	preempt_disable();
	local_irq_enable();

	cpu_set(cpu, cpu_online_map);
	cpu_idle();
}

static int32_t isp_k_yiq_ygamma_index(struct isp_io_param *param)
{
	int32_t ret = 0;
	uint32_t val = 0;
	struct isp_ygamma_node_index ygamma_node_index;

	memset(&ygamma_node_index, 0x00, sizeof(ygamma_node_index));

	ret = copy_from_user((void *)&(ygamma_node_index.node_index), param->property_param, sizeof(ygamma_node_index.node_index));
	if (0 != ret) {
		printk("isp_k_yiq_ygamma_index: copy error, ret=0x%x\n", (uint32_t)ret);
		return -1;
	}

	val = ((ygamma_node_index.node_index[0] & 0x07) << 24)
		| ((ygamma_node_index.node_index[1] & 0x07) << 21)
		| ((ygamma_node_index.node_index[2] & 0x07) << 18)
		| ((ygamma_node_index.node_index[3] & 0x07) << 15)
		| ((ygamma_node_index.node_index[4] & 0x07) << 12)
		| ((ygamma_node_index.node_index[5] & 0x07) << 9)
		| ((ygamma_node_index.node_index[6] & 0x07) << 6)
		| ((ygamma_node_index.node_index[7] & 0x07) << 3)
		| (ygamma_node_index.node_index[8] & 0x07);
	REG_WR(ISP_YGAMMA_NODE_IDX, val);

	return ret;
}

/*
 *	hardware specific access to control-lines
*/
static void crisv32_hwcontrol(struct mtd_info *mtd, int cmd)
{
	unsigned long flags;
	reg_gio_rw_pa_dout dout = REG_RD(gio, regi_gio, rw_pa_dout);

	local_irq_save(flags);
	switch(cmd){
		case NAND_CTL_SETCLE:
		     dout.data |= (1<<CLE_BIT);
		     break;
		case NAND_CTL_CLRCLE:
		     dout.data &= ~(1<<CLE_BIT);
		     break;
		case NAND_CTL_SETALE:
		     dout.data |= (1<<ALE_BIT);
		     break;
		case NAND_CTL_CLRALE:
		     dout.data &= ~(1<<ALE_BIT);
		     break;
		case NAND_CTL_SETNCE:
		     dout.data |= (1<<CE_BIT);
		     break;
		case NAND_CTL_CLRNCE:
		     dout.data &= ~(1<<CE_BIT);
		     break;
	}
	REG_WR(gio, regi_gio, rw_pa_dout, dout);
	local_irq_restore(flags);
}

int crisv32_arbiter_unwatch(int id)
{
	reg_marb_rw_intr_mask intr_mask = REG_RD(marb, regi_marb, rw_intr_mask);

	crisv32_arbiter_init();

	spin_lock(&arbiter_lock);

	if ((id < 0) || (id >= NUMBER_OF_BP) || (!watches[id].used)) {
		spin_unlock(&arbiter_lock);
		return -EINVAL;
	}

	memset(&watches[id], 0, sizeof(struct crisv32_watch_entry));

	if (id == 0)
		intr_mask.bp0 = regk_marb_no;
	else if (id == 1)
		intr_mask.bp1 = regk_marb_no;
	else if (id == 2)
		intr_mask.bp2 = regk_marb_no;
	else if (id == 3)
		intr_mask.bp3 = regk_marb_no;

	REG_WR(marb, regi_marb, rw_intr_mask, intr_mask);

	spin_unlock(&arbiter_lock);
	return 0;
}