diff options
| -rw-r--r-- | drivers/input/rmi4/rmi_smbus.c | 43 |
1 files changed, 18 insertions, 25 deletions
diff --git a/drivers/input/rmi4/rmi_smbus.c b/drivers/input/rmi4/rmi_smbus.c index 6bb67baa1b77..83b1467bb6bf 100644 --- a/drivers/input/rmi4/rmi_smbus.c +++ b/drivers/input/rmi4/rmi_smbus.c @@ -83,63 +83,56 @@ static int rmi_smb_get_command_code(struct rmi_transport_dev *xport, { struct rmi_smb_xport *rmi_smb = container_of(xport, struct rmi_smb_xport, xport); + struct mapping_table_entry new_map; int i; - int retval; - struct mapping_table_entry mapping_data[1]; + int retval = 0; mutex_lock(&rmi_smb->mappingtable_mutex); + for (i = 0; i < RMI_SMB2_MAP_SIZE; i++) { struct mapping_table_entry *entry = &rmi_smb->mapping_table[i]; if (le16_to_cpu(entry->rmiaddr) == rmiaddr) { if (isread) { - if (entry->readcount == bytecount) { - *commandcode = i; - retval = 0; + if (entry->readcount == bytecount) goto exit; - } } else { if (entry->flags & RMI_SMB2_MAP_FLAGS_WE) { - *commandcode = i; - retval = 0; goto exit; } } } } + i = rmi_smb->table_index; rmi_smb->table_index = (i + 1) % RMI_SMB2_MAP_SIZE; /* constructs mapping table data entry. 4 bytes each entry */ - memset(mapping_data, 0, sizeof(mapping_data)); - - mapping_data[0].rmiaddr = cpu_to_le16(rmiaddr); - mapping_data[0].readcount = bytecount; - mapping_data[0].flags = !isread ? RMI_SMB2_MAP_FLAGS_WE : 0; - - retval = smb_block_write(xport, i + 0x80, mapping_data, - sizeof(mapping_data)); + memset(&new_map, 0, sizeof(new_map)); + new_map.rmiaddr = cpu_to_le16(rmiaddr); + new_map.readcount = bytecount; + new_map.flags = !isread ? RMI_SMB2_MAP_FLAGS_WE : 0; + retval = smb_block_write(xport, i + 0x80, &new_map, sizeof(new_map)); if (retval < 0) { /* * if not written to device mapping table * clear the driver mapping table records */ - rmi_smb->mapping_table[i].rmiaddr = 0x0000; - rmi_smb->mapping_table[i].readcount = 0; - rmi_smb->mapping_table[i].flags = 0; - goto exit; + memset(&new_map, 0, sizeof(new_map)); } + /* save to the driver level mapping table */ - rmi_smb->mapping_table[i].rmiaddr = rmiaddr; - rmi_smb->mapping_table[i].readcount = bytecount; - rmi_smb->mapping_table[i].flags = !isread ? RMI_SMB2_MAP_FLAGS_WE : 0; - *commandcode = i; + rmi_smb->mapping_table[i] = new_map; exit: mutex_unlock(&rmi_smb->mappingtable_mutex); - return retval; + if (retval < 0) + return retval; + + *commandcode = i; + return 0; } static int rmi_smb_write_block(struct rmi_transport_dev *xport, u16 rmiaddr, |