static bool motionp_read(struct wiimote_ext *ext)
{
__u8 rmem[2], wmem;
ssize_t ret;
bool avail = false;
if (!atomic_read(&ext->mp_opened))
return false;
if (wiimote_cmd_acquire(ext->wdata))
return false;
/* initialize motion plus */
wmem = 0x55;
ret = wiimote_cmd_write(ext->wdata, 0xa600f0, &wmem, sizeof(wmem));
if (ret)
goto error;
/* read motion plus ID */
ret = wiimote_cmd_read(ext->wdata, 0xa600fe, rmem, 2);
if (ret == 2 || rmem[1] == 0x5)
avail = true;
error:
wiimote_cmd_release(ext->wdata);
return avail;
}
static __u8 ext_read(struct wiimote_ext *ext)
{
ssize_t ret;
__u8 rmem[2], wmem;
__u8 type = WIIEXT_NONE;
if (!ext->plugged || !atomic_read(&ext->opened))
return WIIEXT_NONE;
if (wiimote_cmd_acquire(ext->wdata))
return WIIEXT_NONE;
/* initialize extension */
wmem = 0x55;
ret = wiimote_cmd_write(ext->wdata, 0xa400f0, &wmem, sizeof(wmem));
if (!ret) {
/* disable encryption */
wmem = 0x0;
wiimote_cmd_write(ext->wdata, 0xa400fb, &wmem, sizeof(wmem));
}
/* read extension ID */
ret = wiimote_cmd_read(ext->wdata, 0xa400fe, rmem, 2);
if (ret == 2) {
if (rmem[0] == 0 && rmem[1] == 0)
type = WIIEXT_NUNCHUCK;
else if (rmem[0] == 0x01 && rmem[1] == 0x01)
type = WIIEXT_CLASSIC;
}
wiimote_cmd_release(ext->wdata);
return type;
}
static void ext_enable(struct wiimote_ext *ext, bool motionp, __u8 ext_type)
{
unsigned long flags;
__u8 wmem;
int ret;
if (motionp) {
if (wiimote_cmd_acquire(ext->wdata))
return;
if (ext_type == WIIEXT_CLASSIC)
wmem = 0x07;
else if (ext_type == WIIEXT_NUNCHUCK)
wmem = 0x05;
else
wmem = 0x04;
ret = wiimote_cmd_write(ext->wdata, 0xa600fe, &wmem, sizeof(wmem));
wiimote_cmd_release(ext->wdata);
if (ret)
return;
}
spin_lock_irqsave(&ext->wdata->state.lock, flags);
ext->motionp = motionp;
ext->ext_type = ext_type;
wiiproto_req_drm(ext->wdata, WIIPROTO_REQ_NULL);
spin_unlock_irqrestore(&ext->wdata->state.lock, flags);
}
static ssize_t wiidebug_eeprom_read(struct file *f, char __user *u, size_t s,
loff_t *off)
{
struct wiimote_debug *dbg = f->private_data;
struct wiimote_data *wdata = dbg->wdata;
unsigned long flags;
ssize_t ret;
char buf[16];
__u16 size = 0;
if (s == 0)
return -EINVAL;
if (*off > 0xffffff)
return 0;
if (s > 16)
s = 16;
ret = wiimote_cmd_acquire(wdata);
if (ret)
return ret;
spin_lock_irqsave(&wdata->state.lock, flags);
wdata->state.cmd_read_size = s;
wdata->state.cmd_read_buf = buf;
wiimote_cmd_set(wdata, WIIPROTO_REQ_RMEM, *off & 0xffff);
wiiproto_req_reeprom(wdata, *off, s);
spin_unlock_irqrestore(&wdata->state.lock, flags);
ret = wiimote_cmd_wait(wdata);
if (!ret)
size = wdata->state.cmd_read_size;
spin_lock_irqsave(&wdata->state.lock, flags);
wdata->state.cmd_read_buf = NULL;
spin_unlock_irqrestore(&wdata->state.lock, flags);
wiimote_cmd_release(wdata);
if (ret)
return ret;
else if (size == 0)
return -EIO;
if (copy_to_user(u, buf, size))
return -EFAULT;
*off += size;
ret = size;
return ret;
}
/* diable all extensions */
static void ext_disable(struct wiimote_ext *ext)
{
unsigned long flags;
__u8 wmem = 0x55;
if (!wiimote_cmd_acquire(ext->wdata)) {
wiimote_cmd_write(ext->wdata, 0xa400f0, &wmem, sizeof(wmem));
wiimote_cmd_release(ext->wdata);
}
spin_lock_irqsave(&ext->wdata->state.lock, flags);
ext->motionp = false;
ext->ext_type = WIIEXT_NONE;
wiiproto_req_drm(ext->wdata, WIIPROTO_REQ_NULL);
spin_unlock_irqrestore(&ext->wdata->state.lock, flags);
}
