C++ write_reg函数代码示例

void ili9320_set_window(int x, int y, int xlen, int ylen)
{

    switch(lcd_id)
    {
        case 0x9320:
            write_reg(0x0020, x);
            write_reg(0x0021, y);
            write_reg(0x0050, x);
            write_reg(0x0052, y);
            write_reg(0x0051, x + xlen - 1);
            write_reg(0x0053, y + ylen - 1);  
            break;        
        case 0x8989:
            write_reg(0x004e,x);       
            write_reg(0x004f,y);  
            write_reg(0x0044, (x|((x + xlen - 1)<<8)));
            write_reg(0x0045, y);
            write_reg(0x0046, y + ylen - 1);   
            break;
        default:
            break;   
    }    
} 

static int __init atp_probe1(long ioaddr)
{
	struct net_device *dev = NULL;
	struct net_local *lp;
	int saved_ctrl_reg, status, i;
	int res;

	outb(0xff, ioaddr + PAR_DATA);
	/* Save the original value of the Control register, in case we guessed
	   wrong. */
	saved_ctrl_reg = inb(ioaddr + PAR_CONTROL);
	if (net_debug > 3)
		printk("atp: Control register was %#2.2x.\n", saved_ctrl_reg);
	/* IRQEN=0, SLCTB=high INITB=high, AUTOFDB=high, STBB=high. */
	outb(0x04, ioaddr + PAR_CONTROL);
#ifndef final_version
	if (net_debug > 3) {
		/* Turn off the printer multiplexer on the 8012. */
		for (i = 0; i < 8; i++)
			outb(mux_8012[i], ioaddr + PAR_DATA);
		write_reg(ioaddr, MODSEL, 0x00);
		printk("atp: Registers are ");
		for (i = 0; i < 32; i++)
			printk(" %2.2x", read_nibble(ioaddr, i));
		printk(".\n");
	}
#endif
	/* Turn off the printer multiplexer on the 8012. */
	for (i = 0; i < 8; i++)
		outb(mux_8012[i], ioaddr + PAR_DATA);
	write_reg_high(ioaddr, CMR1, CMR1h_RESET);
	/* udelay() here? */
	status = read_nibble(ioaddr, CMR1);

	if (net_debug > 3) {
		printk(KERN_DEBUG "atp: Status nibble was %#2.2x..", status);
		for (i = 0; i < 32; i++)
			printk(" %2.2x", read_nibble(ioaddr, i));
		printk("\n");
	}

	if ((status & 0x78) != 0x08) {
		/* The pocket adapter probe failed, restore the control register. */
		outb(saved_ctrl_reg, ioaddr + PAR_CONTROL);
		return -ENODEV;
	}
	status = read_nibble(ioaddr, CMR2_h);
	if ((status & 0x78) != 0x10) {
		outb(saved_ctrl_reg, ioaddr + PAR_CONTROL);
		return -ENODEV;
	}

	dev = alloc_etherdev(sizeof(struct net_local));
	if (!dev)
		return -ENOMEM;

	/* Find the IRQ used by triggering an interrupt. */
	write_reg_byte(ioaddr, CMR2, 0x01);			/* No accept mode, IRQ out. */
	write_reg_high(ioaddr, CMR1, CMR1h_RxENABLE | CMR1h_TxENABLE);	/* Enable Tx and Rx. */

	/* Omit autoIRQ routine for now. Use "table lookup" instead.  Uhgggh. */
	if (irq[0])
		dev->irq = irq[0];
	else if (ioaddr == 0x378)
		dev->irq = 7;
	else
		dev->irq = 5;
	write_reg_high(ioaddr, CMR1, CMR1h_TxRxOFF); /* Disable Tx and Rx units. */
	write_reg(ioaddr, CMR2, CMR2_NULL);

	dev->base_addr = ioaddr;

	/* Read the station address PROM.  */
	get_node_ID(dev);

#ifndef MODULE
	if (net_debug)
		printk(KERN_INFO "%s", version);
#endif

	printk(KERN_NOTICE "%s: Pocket adapter found at %#3lx, IRQ %d, "
	       "SAPROM %pM.\n",
	       dev->name, dev->base_addr, dev->irq, dev->dev_addr);

	/* Reset the ethernet hardware and activate the printer pass-through. */
	write_reg_high(ioaddr, CMR1, CMR1h_RESET | CMR1h_MUX);

	lp = netdev_priv(dev);
	lp->chip_type = RTL8002;
	lp->addr_mode = CMR2h_Normal;
	spin_lock_init(&lp->lock);

	/* For the ATP adapter the "if_port" is really the data transfer mode. */
	if (xcvr[0])
		dev->if_port = xcvr[0];
	else
		dev->if_port = (dev->mem_start & 0xf) ? (dev->mem_start & 0x7) : 4;
	if (dev->mem_end & 0xf)
		net_debug = dev->mem_end & 7;

//.........这里部分代码省略.........

static void set_addr_win(struct fbtft_par *par, int xs, int ys, int xe, int ye)
{
	write_reg(par, 0x15, xs, xe);
	write_reg(par, 0x75, ys, ye);
	write_reg(par, 0x5c);
}

/*
 * Generic i2c master transfer entrypoint
 */
static int
keywest_xfer(	struct i2c_adapter *adap,
		struct i2c_msg msgs[], 
		int num)
{
	struct keywest_chan* chan = i2c_get_adapdata(adap);
	struct keywest_iface* iface = chan->iface;
	struct i2c_msg *pmsg;
	int i, completed;
	int rc = 0;

	down(&iface->sem);

	/* Set adapter to standard mode */
	iface->cur_mode &= ~KW_I2C_MODE_MODE_MASK;
	iface->cur_mode |= KW_I2C_MODE_STANDARD;

	completed = 0;
	for (i = 0; rc >= 0 && i < num;) {
		u8 addr;
		
		pmsg = &msgs[i++];
		addr = pmsg->addr;
		if (pmsg->flags & I2C_M_TEN) {
			printk(KERN_ERR "i2c-keywest: 10 bits addr not supported !\n");
			rc = -EINVAL;
			break;
		}
		DBG("xfer: chan: %d, doing %s %d bytes to 0x%02x - %d of %d messages\n",
		     chan->chan_no,
		     pmsg->flags & I2C_M_RD ? "read" : "write",
                     pmsg->len, addr, i, num);
    
		/* Setup channel & clear pending irqs */
		write_reg(reg_mode, iface->cur_mode | (chan->chan_no << 4));
		write_reg(reg_isr, read_reg(reg_isr));
		write_reg(reg_status, 0);
		
		iface->data = pmsg->buf;
		iface->datalen = pmsg->len;
		iface->state = state_addr;
		iface->result = 0;
		iface->stopretry = 0;
		if (pmsg->flags & I2C_M_RD)
			iface->read_write = I2C_SMBUS_READ;
		else
			iface->read_write = I2C_SMBUS_WRITE;

		/* Set up address and r/w bit */
		if (pmsg->flags & I2C_M_REV_DIR_ADDR)
			addr ^= 1;		
		write_reg(reg_addr,
			(addr << 1) |
			((iface->read_write == I2C_SMBUS_READ) ? 0x01 : 0x00));

		/* Arm timeout */
		mod_timer(&iface->timeout_timer, jiffies + POLL_TIMEOUT);

		/* Start sending address & enable interrupt*/
		write_reg(reg_control, read_reg(reg_control) | KW_I2C_CTL_XADDR);
		write_reg(reg_ier, KW_I2C_IRQ_MASK);

		/* Wait interrupt operations completion */
		wait_for_completion(&iface->complete);	

		rc = iface->result;
		if (rc == 0)
			completed++;
		DBG("transfer done, result: %d\n", rc);
	}

	/* Release sem */
	up(&iface->sem);

	return completed;
}

/* Main state machine for standard & standard sub mode */
static int
handle_interrupt(struct keywest_iface *iface, u8 isr)
{
	int ack;
	int rearm_timer = 1;
	
	DBG("handle_interrupt(), got: %x, status: %x, state: %d\n",
		isr, read_reg(reg_status), iface->state);
	if (isr == 0 && iface->state != state_stop) {
		do_stop(iface, -1);
		return rearm_timer;
	}
	if (isr & KW_I2C_IRQ_STOP && iface->state != state_stop) {
		iface->result = -1;
		iface->state = state_stop;
	}
	switch(iface->state) {
	case state_addr:
		if (!(isr & KW_I2C_IRQ_ADDR)) {
			do_stop(iface, -1);
			break;
		}
		ack = read_reg(reg_status);
		DBG("ack on set address: %x\n", ack);
		if ((ack & KW_I2C_STAT_LAST_AAK) == 0) {
			do_stop(iface, -1);
			break;
		}
		/* Handle rw "quick" mode */
		if (iface->datalen == 0)
			do_stop(iface, 0);
		else if (iface->read_write == I2C_SMBUS_READ) {
			iface->state = state_read;
			if (iface->datalen > 1)
				write_reg(reg_control, read_reg(reg_control)
					| KW_I2C_CTL_AAK);
		} else {
			iface->state = state_write;
			DBG("write byte: %x\n", *(iface->data));
			write_reg(reg_data, *(iface->data++));
			iface->datalen--;
		}
		
		break;
	case state_read:
		if (!(isr & KW_I2C_IRQ_DATA)) {
			do_stop(iface, -1);
			break;
		}
		*(iface->data++) = read_reg(reg_data);
		DBG("read byte: %x\n", *(iface->data-1));
		iface->datalen--;
		if (iface->datalen == 0)
			iface->state = state_stop;
		else
			write_reg(reg_control, 0);
		break;
	case state_write:
		if (!(isr & KW_I2C_IRQ_DATA)) {
			do_stop(iface, -1);
			break;
		}
		/* Check ack status */
		ack = read_reg(reg_status);
		DBG("ack on data write: %x\n", ack);
		if ((ack & KW_I2C_STAT_LAST_AAK) == 0) {
			do_stop(iface, -1);
			break;
		}
		if (iface->datalen) {
			DBG("write byte: %x\n", *(iface->data));
			write_reg(reg_data, *(iface->data++));
			iface->datalen--;
		} else
			do_stop(iface, 0);
		break;
		
	case state_stop:
		if (!(isr & KW_I2C_IRQ_STOP) && (++iface->stopretry) < 10)
			do_stop(iface, -1);
		else {
			rearm_timer = 0;
			iface->state = state_idle;
			write_reg(reg_control, 0x00);
			write_reg(reg_ier, 0x00);
			complete(&iface->complete);
		}
		break;
	}
	
	write_reg(reg_isr, isr);

	return rearm_timer;
}

//Clear FIFO Complete flag
void ArduCAM::clear_fifo_flag(void)
{
	write_reg(ARDUCHIP_FIFO, FIFO_CLEAR_MASK);
}

int
LSM303D::init()
{
    int ret = ERROR;
    int mag_ret;
    int fd_mag;

    /* do SPI init (and probe) first */
    if (SPI::init() != OK)
        goto out;

    /* allocate basic report buffers */
    _num_accel_reports = 2;
    _oldest_accel_report = _next_accel_report = 0;
    _accel_reports = new struct accel_report[_num_accel_reports];

    if (_accel_reports == nullptr)
        goto out;

    /* advertise accel topic */
    memset(&_accel_reports[0], 0, sizeof(_accel_reports[0]));
    _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]);

    _num_mag_reports = 2;
    _oldest_mag_report = _next_mag_report = 0;
    _mag_reports = new struct mag_report[_num_mag_reports];

    if (_mag_reports == nullptr)
        goto out;

    /* advertise mag topic */
    memset(&_mag_reports[0], 0, sizeof(_mag_reports[0]));
    _mag_topic = orb_advertise(ORB_ID(sensor_mag), &_mag_reports[0]);

    /* enable accel, XXX do this with an ioctl? */
    write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A);

    /* enable mag, XXX do this with an ioctl? */
    write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
    write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);

    /* XXX should we enable FIFO? */

    set_range(8); /* XXX 16G mode seems wrong (shows 6 instead of 9.8m/s^2, therefore use 8G for now */
    set_antialias_filter_bandwidth(50); /* available bandwidths: 50, 194, 362 or 773 Hz */
    set_samplerate(400); /* max sample rate */

    mag_set_range(4); /* XXX: take highest sensor range of 12GA? */
    mag_set_samplerate(100);

    /* XXX test this when another mag is used */
    /* do CDev init for the mag device node, keep it optional */
    mag_ret = _mag->init();

    if (mag_ret != OK) {
        _mag_topic = -1;
    }

    ret = OK;
out:
    return ret;
}

int MPU9250::reset()
{
	write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
	up_udelay(10000);

	write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_AUTO);
	up_udelay(1000);

	write_checked_reg(MPUREG_PWR_MGMT_2, 0);
	up_udelay(1000);

	// SAMPLE RATE
	_set_sample_rate(_sample_rate);
	usleep(1000);

	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
	// was 90 Hz, but this ruins quality and does not improve the
	// system response
	_set_dlpf_filter(MPU9250_DEFAULT_ONCHIP_FILTER_FREQ);
	usleep(1000);

	// Gyro scale 2000 deg/s ()
	write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
	usleep(1000);

	// correct gyro scale factors
	// scale to rad/s in SI units
	// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
	// scaling factor:
	// 1/(2^15)*(2000/180)*PI
	_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
	_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;

	set_accel_range(16);

	usleep(1000);

	// INT CFG => Interrupt on Data Ready
	write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN);        // INT: Raw data ready
	usleep(1000);
	write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read
	usleep(1000);

	uint8_t retries = 10;

	while (retries--) {
		bool all_ok = true;

		for (uint8_t i = 0; i < MPU9250_NUM_CHECKED_REGISTERS; i++) {
			if (read_reg(_checked_registers[i]) != _checked_values[i]) {
				write_reg(_checked_registers[i], _checked_values[i]);
				all_ok = false;
			}
		}

		if (all_ok) {
			break;
		}
	}

	return OK;
}

static int __init atp_probe1(long ioaddr)
{
	struct net_device *dev = NULL;
	struct net_local *lp;
	int saved_ctrl_reg, status, i;
	int res;

	outb(0xff, ioaddr + PAR_DATA);
	saved_ctrl_reg = inb(ioaddr + PAR_CONTROL);
	if (net_debug > 3)
		printk("atp: Control register was %#2.2x.\n", saved_ctrl_reg);
	
	outb(0x04, ioaddr + PAR_CONTROL);
#ifndef final_version
	if (net_debug > 3) {
		
		for (i = 0; i < 8; i++)
			outb(mux_8012[i], ioaddr + PAR_DATA);
		write_reg(ioaddr, MODSEL, 0x00);
		printk("atp: Registers are ");
		for (i = 0; i < 32; i++)
			printk(" %2.2x", read_nibble(ioaddr, i));
		printk(".\n");
	}
#endif
	
	for (i = 0; i < 8; i++)
		outb(mux_8012[i], ioaddr + PAR_DATA);
	write_reg_high(ioaddr, CMR1, CMR1h_RESET);
	
	status = read_nibble(ioaddr, CMR1);

	if (net_debug > 3) {
		printk(KERN_DEBUG "atp: Status nibble was %#2.2x..", status);
		for (i = 0; i < 32; i++)
			printk(" %2.2x", read_nibble(ioaddr, i));
		printk("\n");
	}

	if ((status & 0x78) != 0x08) {
		
		outb(saved_ctrl_reg, ioaddr + PAR_CONTROL);
		return -ENODEV;
	}
	status = read_nibble(ioaddr, CMR2_h);
	if ((status & 0x78) != 0x10) {
		outb(saved_ctrl_reg, ioaddr + PAR_CONTROL);
		return -ENODEV;
	}

	dev = alloc_etherdev(sizeof(struct net_local));
	if (!dev)
		return -ENOMEM;

	
	write_reg_byte(ioaddr, CMR2, 0x01);			
	write_reg_high(ioaddr, CMR1, CMR1h_RxENABLE | CMR1h_TxENABLE);	

	
	if (irq[0])
		dev->irq = irq[0];
	else if (ioaddr == 0x378)
		dev->irq = 7;
	else
		dev->irq = 5;
	write_reg_high(ioaddr, CMR1, CMR1h_TxRxOFF); 
	write_reg(ioaddr, CMR2, CMR2_NULL);

	dev->base_addr = ioaddr;

	
	get_node_ID(dev);

#ifndef MODULE
	if (net_debug)
		printk(KERN_INFO "%s", version);
#endif

	printk(KERN_NOTICE "%s: Pocket adapter found at %#3lx, IRQ %d, "
	       "SAPROM %pM.\n",
	       dev->name, dev->base_addr, dev->irq, dev->dev_addr);

	
	write_reg_high(ioaddr, CMR1, CMR1h_RESET | CMR1h_MUX);

	lp = netdev_priv(dev);
	lp->chip_type = RTL8002;
	lp->addr_mode = CMR2h_Normal;
	spin_lock_init(&lp->lock);

	
	if (xcvr[0])
		dev->if_port = xcvr[0];
	else
		dev->if_port = (dev->mem_start & 0xf) ? (dev->mem_start & 0x7) : 4;
	if (dev->mem_end & 0xf)
		net_debug = dev->mem_end & 7;

	dev->netdev_ops 	= &atp_netdev_ops;
	dev->watchdog_timeo	= TX_TIMEOUT;
//.........这里部分代码省略.........

static int wis_tw2804_command(struct i2c_client *client,
				unsigned int cmd, void *arg)
{
	struct wis_tw2804 *dec = i2c_get_clientdata(client);

	if (cmd == DECODER_SET_CHANNEL) {
		int *input = arg;

		if (*input < 0 || *input > 3) {
			printk(KERN_ERR "wis-tw2804: channel %d is not "
					"between 0 and 3!\n", *input);
			return 0;
		}
		dec->channel = *input;
		printk(KERN_DEBUG "wis-tw2804: initializing TW2804 "
				"channel %d\n", dec->channel);
		if (dec->channel == 0 &&
				write_regs(client, global_registers, 0) < 0)
		{
			printk(KERN_ERR "wis-tw2804: error initializing "
					"TW2804 global registers\n");
			return 0;
		}
		if (write_regs(client, channel_registers, dec->channel) < 0)
		{
			printk(KERN_ERR "wis-tw2804: error initializing "
					"TW2804 channel %d\n", dec->channel);
			return 0;
		}
		return 0;
	}

	if (dec->channel < 0) {
		printk(KERN_DEBUG "wis-tw2804: ignoring command %08x until "
				"channel number is set\n", cmd);
		return 0;
	}

	switch (cmd) {
	case DECODER_SET_NORM:
	{
		int *input = arg;
		u8 regs[] = {
			0x01, *input == VIDEO_MODE_NTSC ? 0xc4 : 0x84,
			0x09, *input == VIDEO_MODE_NTSC ? 0x07 : 0x04,
			0x0a, *input == VIDEO_MODE_NTSC ? 0xf0 : 0x20,
			0x0b, *input == VIDEO_MODE_NTSC ? 0x07 : 0x04,
			0x0c, *input == VIDEO_MODE_NTSC ? 0xf0 : 0x20,
			0x0d, *input == VIDEO_MODE_NTSC ? 0x40 : 0x4a,
			0x16, *input == VIDEO_MODE_NTSC ? 0x00 : 0x40,
			0x17, *input == VIDEO_MODE_NTSC ? 0x00 : 0x40,
			0x20, *input == VIDEO_MODE_NTSC ? 0x07 : 0x0f,
			0x21, *input == VIDEO_MODE_NTSC ? 0x07 : 0x0f,
			0xff,	0xff,
		};
		write_regs(client, regs, dec->channel);
		dec->norm = *input;
		break;
	}
	case VIDIOC_QUERYCTRL:
	{
		struct v4l2_queryctrl *ctrl = arg;

		switch (ctrl->id) {
		case V4L2_CID_BRIGHTNESS:
			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
			strncpy(ctrl->name, "Brightness", sizeof(ctrl->name));
			ctrl->minimum = 0;
			ctrl->maximum = 255;
			ctrl->step = 1;
			ctrl->default_value = 128;
			ctrl->flags = 0;
			break;
		case V4L2_CID_CONTRAST:
			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
			strncpy(ctrl->name, "Contrast", sizeof(ctrl->name));
			ctrl->minimum = 0;
			ctrl->maximum = 255;
			ctrl->step = 1;
			ctrl->default_value = 128;
			ctrl->flags = 0;
			break;
		case V4L2_CID_SATURATION:
			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
			strncpy(ctrl->name, "Saturation", sizeof(ctrl->name));
			ctrl->minimum = 0;
			ctrl->maximum = 255;
			ctrl->step = 1;
			ctrl->default_value = 128;
			ctrl->flags = 0;
			break;
		case V4L2_CID_HUE:
			ctrl->type = V4L2_CTRL_TYPE_INTEGER;
			strncpy(ctrl->name, "Hue", sizeof(ctrl->name));
			ctrl->minimum = 0;
			ctrl->maximum = 255;
			ctrl->step = 1;
			ctrl->default_value = 128;
			ctrl->flags = 0;
			break;
//.........这里部分代码省略.........

static int init_display(struct fbtft_par *par)
{
	par->fbtftops.reset(par);

	/* softreset of LCD */
	write_reg(par, LCD_RESET_CMD);
	mdelay(10);

	/* set startpoint */
	/* LCD_START_LINE | (pos & 0x3F) */
	write_reg(par, LCD_START_LINE);

	/* select orientation BOTTOMVIEW */
	write_reg(par, LCD_BOTTOMVIEW | 1);
	/* output mode select (turns display upside-down) */
	write_reg(par, LCD_SCAN_DIR | 0x00);

	/* Normal Pixel mode */
	write_reg(par, LCD_ALL_PIXEL | 0);

	/* positive display */
	write_reg(par, LCD_DISPLAY_INVERT | 0);

	/* bias 1/9 */
	write_reg(par, LCD_BIAS | 0);

	/* power control mode: all features on */
	/* LCD_POWER_CONTROL | (val&0x07) */
	write_reg(par, LCD_POWER_CONTROL | 0x07);

	/* set voltage regulator R/R */
	/* LCD_VOLTAGE | (val&0x07) */
	write_reg(par, LCD_VOLTAGE | 0x07);

	/* volume mode set */
	/* LCD_VOLUME_MODE,val&0x3f,LCD_NO_OP */
	write_reg(par, LCD_VOLUME_MODE);
	/* LCD_VOLUME_MODE,val&0x3f,LCD_NO_OP */
	write_reg(par, 0x09);
	/* ???? */
	/* LCD_VOLUME_MODE,val&0x3f,LCD_NO_OP */
	write_reg(par, LCD_NO_OP);

	/* advanced program control */
	write_reg(par, LCD_ADV_PROG_CTRL);
	write_reg(par, LCD_ADV_PROG_CTRL2 | LCD_TEMPCOMP_HIGH);

	/* enable display */
	write_reg(par, LCD_DISPLAY_ENABLE | 1);

	return 0;
}

static void ivtv_yuv_filter(struct ivtv *itv, int h_filter, int v_filter_1, int v_filter_2)
{
    u32 i, line;

    /* If any filter is -1, then don't update it */
    if (h_filter > -1) {
        if (h_filter > 4)
            h_filter = 4;
        i = IVTV_YUV_HORIZONTAL_FILTER_OFFSET + (h_filter * 384);
        for (line = 0; line < 16; line++) {
            write_reg(read_dec(i), 0x02804);
            write_reg(read_dec(i), 0x0281c);
            i += 4;
            write_reg(read_dec(i), 0x02808);
            write_reg(read_dec(i), 0x02820);
            i += 4;
            write_reg(read_dec(i), 0x0280c);
            write_reg(read_dec(i), 0x02824);
            i += 4;
            write_reg(read_dec(i), 0x02810);
            write_reg(read_dec(i), 0x02828);
            i += 4;
            write_reg(read_dec(i), 0x02814);
            write_reg(read_dec(i), 0x0282c);
            i += 8;
            write_reg(0, 0x02818);
            write_reg(0, 0x02830);
        }
        IVTV_DEBUG_YUV("h_filter -> %d\n", h_filter);
    }

    if (v_filter_1 > -1) {
        if (v_filter_1 > 4)
            v_filter_1 = 4;
        i = IVTV_YUV_VERTICAL_FILTER_OFFSET + (v_filter_1 * 192);
        for (line = 0; line < 16; line++) {
            write_reg(read_dec(i), 0x02900);
            i += 4;
            write_reg(read_dec(i), 0x02904);
            i += 8;
            write_reg(0, 0x02908);
        }
        IVTV_DEBUG_YUV("v_filter_1 -> %d\n", v_filter_1);
    }

    if (v_filter_2 > -1) {
        if (v_filter_2 > 4)
            v_filter_2 = 4;
        i = IVTV_YUV_VERTICAL_FILTER_OFFSET + (v_filter_2 * 192);
        for (line = 0; line < 16; line++) {
            write_reg(read_dec(i), 0x0290c);
            i += 4;
            write_reg(read_dec(i), 0x02910);
            i += 8;
            write_reg(0, 0x02914);
        }
        IVTV_DEBUG_YUV("v_filter_2 -> %d\n", v_filter_2);
    }
}

void lcd_SetCursor(unsigned int x,unsigned int y)
{
	write_reg(0x004e,x);    /* 0-239 */
	write_reg(0x004f,y);    /* 0-319 */
}

void lcd_init(void)
{
	port_fsmc_init();
	//GPIO_ResetBits(GPIOD,GPIO_Pin_3);	
	//delay(500);
	//GPIO_SetBits(GPIOD,GPIO_Pin_3);	
	//delay(50);
	GPIO_SetBits(GPIOF,GPIO_Pin_9);
	delay(50);
	DeviceCode = read_reg(0x0000);
	//if (DeviceCode == 0x8999)
	{
		// power supply setting
		// set R07h at 0021h (GON=1,DTE=0,D[1:0]=01)
		write_reg(0x07,0x0021);
		// set R00h at 0001h (OSCEN=1)
		write_reg(0x00,0x0001);
		// set R07h at 0023h (GON=1,DTE=0,D[1:0]=11)
		write_reg(0x07,0x0023);
		// set R10h at 0000h (Exit sleep mode)
		write_reg(0x10,0x0000);
		// Wait 30ms
		delay(3000);
		// set R07h at 0033h (GON=1,DTE=1,D[1:0]=11)
		write_reg(0x07,0x0033);
		// Entry mode setting (R11h)
		// R11H Entry mode
		// vsmode DFM1 DFM0 TRANS OEDef WMode DMode1 DMode0 TY1 TY0 ID1 ID0 AM LG2 LG2 LG0
		//   0     1    1     0     0     0     0      0     0   1   1   1  *   0   0   0
		write_reg(0x11,0x6078);
		// LCD driver AC setting (R02h)
		write_reg(0x02,0x0600);
		// power control 1
		// DCT3 DCT2 DCT1 DCT0 BT2 BT1 BT0 0 DC3 DC2 DC1 DC0 AP2 AP1 AP0 0
		// 1     0    1    0    1   0   0  0  1   0   1   0   0   1   0  0
		// DCT[3:0] fosc/4 BT[2:0]  DC{3:0] fosc/4
		write_reg(0x03,0x0804);//0xA8A4
		write_reg(0x0C,0x0000);//
		write_reg(0x0D,0x080C);//
		// power control 4
		// 0 0 VCOMG VDV4 VDV3 VDV2 VDV1 VDV0 0 0 0 0 0 0 0 0
		// 0 0   1    0    1    0    1    1   0 0 0 0 0 0 0 0
		write_reg(0x0E,0x2900);
		write_reg(0x1E,0x00B8);
		write_reg(0x01,0x2B3F);//驱动输出控制320*240  0x2B3F           0x6B3F
		write_reg(0x10,0x0000);                                  
		write_reg(0x05,0x0000);
		write_reg(0x06,0x0000);
		write_reg(0x16,0xEF1C);
		write_reg(0x17,0x0003);
		write_reg(0x07,0x0233);//0x0233
		write_reg(0x0B,0x0000|(3<<6));
		write_reg(0x0F,0x0000);//扫描开始地址
		write_reg(0x41,0x0000);
		write_reg(0x42,0x0000);
		write_reg(0x48,0x0000);
		write_reg(0x49,0x013F);
		write_reg(0x4A,0x0000);
		write_reg(0x4B,0x0000);
		write_reg(0x44,0xEF00);
		write_reg(0x45,0x0000);
		write_reg(0x46,0x013F);

//		write_reg(0x44,0x7700);
//		write_reg(0x45,0x0000);
//		write_reg(0x46,0x09f);

		write_reg(0x30,0x0707);
		write_reg(0x31,0x0204);
		write_reg(0x32,0x0204);
		write_reg(0x33,0x0502);
		write_reg(0x34,0x0507);
		write_reg(0x35,0x0204);
		write_reg(0x36,0x0204);
		write_reg(0x37,0x0502);
		write_reg(0x3A,0x0302);
		write_reg(0x3B,0x0302);
		write_reg(0x23,0x0000);
		write_reg(0x24,0x0000);
		write_reg(0x25,0x8000);   // 65hz
		write_reg(0x4f,0);        // 行首址0
		write_reg(0x4e,0);        // 列首址0	

	}	
	//else
	//{
		//printf("LCD model is not recognized,DeviceCode = 0x%x!\r\n",DeviceCode);
		//return;
	//}
	//数据总线测试,用于测试硬件连接是否正常.
	lcd_data_bus_test();
	lcd_clear(Red);	
	kprintf("LCD Init Finish\n\r");
}

/* Init sequence taken from: Arduino Library for the Adafruit 2.2" display */
static int init_display(struct fbtft_par *par)
{
	fbtft_par_dbg(DEBUG_INIT_DISPLAY, par, "%s()\n", __func__);

	par->fbtftops.reset(par);

	write_reg(par, 0xEF, 0x03, 0x80, 0x02);
	write_reg(par, 0xCF, 0x00 , 0XC1 , 0X30);
	write_reg(par, 0xED, 0x64 , 0x03 , 0X12 , 0X81);
	write_reg(par, 0xE8, 0x85 , 0x00 , 0x78);
	write_reg(par, 0xCB, 0x39 , 0x2C , 0x00 , 0x34 , 0x02);
	write_reg(par, 0xF7, 0x20);
	write_reg(par, 0xEA, 0x00 , 0x00);

	/* Power Control 1 */
	write_reg(par, 0xC0, 0x23);

	/* Power Control 2 */
	write_reg(par, 0xC1, 0x10);

	/* VCOM Control 1 */
	write_reg(par, 0xC5, 0x3e, 0x28);

	/* VCOM Control 2 */
	write_reg(par, 0xC7, 0x86);

	/* COLMOD: Pixel Format Set */
	/* 16 bits/pixel */
	write_reg(par, 0x3A, 0x55);

	/* Frame Rate Control */
	/* Division ratio = fosc, Frame Rate = 79Hz */
	write_reg(par, 0xB1, 0x00, 0x18);

	/* Display Function Control */
	write_reg(par, 0xB6, 0x08, 0x82, 0x27);

	/* Gamma Function Disable */
	write_reg(par, 0xF2, 0x00);

	/* Gamma curve selected  */
	write_reg(par, 0x26, 0x01);

	/* Positive Gamma Correction */
	write_reg(par, 0xE0,
		0x0F, 0x31, 0x2B, 0x0C, 0x0E, 0x08, 0x4E, 0xF1,
		0x37, 0x07, 0x10, 0x03, 0x0E, 0x09, 0x00);

	/* Negative Gamma Correction */
	write_reg(par, 0xE1,
		0x00, 0x0E, 0x14, 0x03, 0x11, 0x07, 0x31, 0xC1,
		0x48, 0x08, 0x0F, 0x0C, 0x31, 0x36, 0x0F);

	/* Sleep OUT */
	write_reg(par, 0x11);

	mdelay(120);

	/* Display ON */
	write_reg(par, 0x29);

	return 0;
}