/* * __wait_latch - Wait up to 100us for a port latch to get a certain value, * returning zero if the value is obtained. Callers must hold hdaps_sem. */ static int __wait_latch(u16 port, u8 val) { unsigned int i; for (i = 0; i < 20; i++) { if (!__check_latch(port, val)) return 0; udelay(5); } return -EIO; }
/* * hdaps_device_init - initialize the accelerometer. Returns zero on success * and negative error code on failure. Can sleep. */ static int hdaps_device_init(void) { int total, ret = -ENXIO; down(&hdaps_sem); outb(0x13, 0x1610); outb(0x01, 0x161f); if (__wait_latch(0x161f, 0x00)) goto out; /* * Most ThinkPads return 0x01. * * Others--namely the R50p, T41p, and T42p--return 0x03. These laptops * have "inverted" axises. * * The 0x02 value occurs when the chip has been previously initialized. */ if (__check_latch(0x1611, 0x03) && __check_latch(0x1611, 0x02) && __check_latch(0x1611, 0x01)) goto out; printk(KERN_DEBUG "hdaps: initial latch check good (0x%02x).\n", __get_latch(0x1611)); outb(0x17, 0x1610); outb(0x81, 0x1611); outb(0x01, 0x161f); if (__wait_latch(0x161f, 0x00)) goto out; if (__wait_latch(0x1611, 0x00)) goto out; if (__wait_latch(0x1612, 0x60)) goto out; if (__wait_latch(0x1613, 0x00)) goto out; outb(0x14, 0x1610); outb(0x01, 0x1611); outb(0x01, 0x161f); if (__wait_latch(0x161f, 0x00)) goto out; outb(0x10, 0x1610); outb(0xc8, 0x1611); outb(0x00, 0x1612); outb(0x02, 0x1613); outb(0x01, 0x161f); if (__wait_latch(0x161f, 0x00)) goto out; if (__device_refresh_sync()) goto out; if (__wait_latch(0x1611, 0x00)) goto out; /* we have done our dance, now let's wait for the applause */ for (total = INIT_TIMEOUT_MSECS; total > 0; total -= INIT_WAIT_MSECS) { int x, y; /* a read of the device helps push it into action */ __hdaps_read_pair(HDAPS_PORT_XPOS, HDAPS_PORT_YPOS, &x, &y); if (!__wait_latch(0x1611, 0x02)) { ret = 0; break; } msleep(INIT_WAIT_MSECS); } out: up(&hdaps_sem); return ret; }
static int hdaps_device_init(void) { int total, ret = -ENXIO; mutex_lock(&hdaps_mtx); outb(0x13, 0x1610); outb(0x01, 0x161f); if (__wait_latch(0x161f, 0x00)) goto out; if (__check_latch(0x1611, 0x03) && __check_latch(0x1611, 0x02) && __check_latch(0x1611, 0x01)) goto out; printk(KERN_DEBUG "hdaps: initial latch check good (0x%02x).\n", __get_latch(0x1611)); outb(0x17, 0x1610); outb(0x81, 0x1611); outb(0x01, 0x161f); if (__wait_latch(0x161f, 0x00)) goto out; if (__wait_latch(0x1611, 0x00)) goto out; if (__wait_latch(0x1612, 0x60)) goto out; if (__wait_latch(0x1613, 0x00)) goto out; outb(0x14, 0x1610); outb(0x01, 0x1611); outb(0x01, 0x161f); if (__wait_latch(0x161f, 0x00)) goto out; outb(0x10, 0x1610); outb(0xc8, 0x1611); outb(0x00, 0x1612); outb(0x02, 0x1613); outb(0x01, 0x161f); if (__wait_latch(0x161f, 0x00)) goto out; if (__device_refresh_sync()) goto out; if (__wait_latch(0x1611, 0x00)) goto out; for (total = INIT_TIMEOUT_MSECS; total > 0; total -= INIT_WAIT_MSECS) { int x, y; __hdaps_read_pair(HDAPS_PORT_XPOS, HDAPS_PORT_YPOS, &x, &y); if (!__wait_latch(0x1611, 0x02)) { ret = 0; break; } msleep(INIT_WAIT_MSECS); } out: mutex_unlock(&hdaps_mtx); return ret; }