Andrew Duggan | 4e81125 | 2014-04-03 15:17:57 -0700 | [diff] [blame] | 1 | #include <stdio.h> |
| 2 | #include <time.h> |
| 3 | #include <string.h> |
| 4 | #include <errno.h> |
| 5 | |
| 6 | #include "rmidevice.h" |
| 7 | |
| 8 | #define RMI_DEVICE_PDT_ENTRY_SIZE 6 |
| 9 | #define RMI_DEVICE_PAGE_SELECT_REGISTER 0xFF |
| 10 | #define RMI_DEVICE_MAX_PAGE 0xFF |
| 11 | #define RMI_DEVICE_PAGE_SIZE 0x100 |
| 12 | #define RMI_DEVICE_PAGE_SCAN_START 0x00e9 |
| 13 | #define RMI_DEVICE_PAGE_SCAN_END 0x0005 |
| 14 | #define RMI_DEVICE_F01_BASIC_QUERY_LEN 21 |
| 15 | #define RMI_DEVICE_F01_PRODUCTINFO_MASK 0x7f |
| 16 | #define RMI_DEVICE_F01_QRY5_YEAR_MASK 0x1f |
| 17 | #define RMI_DEVICE_F01_QRY6_MONTH_MASK 0x0f |
| 18 | #define RMI_DEVICE_F01_QRY7_DAY_MASK 0x1f |
| 19 | |
| 20 | #define RMI_DEVICE_F01_QRY1_HAS_LTS (1 << 2) |
| 21 | #define RMI_DEVICE_F01_QRY1_HAS_SENSOR_ID (1 << 3) |
| 22 | #define RMI_DEVICE_F01_QRY1_HAS_CHARGER_INP (1 << 4) |
| 23 | #define RMI_DEVICE_F01_QRY1_HAS_ADJ_DOZE (1 << 5) |
| 24 | #define RMI_DEVICE_F01_QRY1_HAS_ADJ_DOZE_HOFF (1 << 6) |
| 25 | #define RMI_DEVICE_F01_QRY1_HAS_PROPS_2 (1 << 7) |
| 26 | |
| 27 | #define RMI_DEVICE_F01_LTS_RESERVED_SIZE 19 |
| 28 | |
| 29 | #define RMI_DEVICE_F01_QRY42_DS4_QUERIES (1 << 0) |
| 30 | #define RMI_DEVICE_F01_QRY42_MULTI_PHYS (1 << 1) |
| 31 | |
| 32 | #define RMI_DEVICE_F01_QRY43_01_PACKAGE_ID (1 << 0) |
| 33 | #define RMI_DEVICE_F01_QRY43_01_BUILD_ID (1 << 1) |
| 34 | |
| 35 | #define PACKAGE_ID_BYTES 4 |
| 36 | #define BUILD_ID_BYTES 3 |
| 37 | |
| 38 | #define RMI_F01_CMD_DEVICE_RESET 1 |
| 39 | #define RMI_F01_DEFAULT_RESET_DELAY_MS 100 |
| 40 | |
| 41 | int RMIDevice::SetRMIPage(unsigned char page) |
| 42 | { |
| 43 | return Write(RMI_DEVICE_PAGE_SELECT_REGISTER, &page, 1); |
| 44 | } |
| 45 | |
| 46 | int RMIDevice::QueryBasicProperties() |
| 47 | { |
| 48 | int rc; |
| 49 | unsigned char basicQuery[RMI_DEVICE_F01_BASIC_QUERY_LEN]; |
| 50 | unsigned short queryAddr; |
| 51 | unsigned char infoBuf[4]; |
| 52 | unsigned short prodInfoAddr; |
| 53 | RMIFunction f01; |
| 54 | |
| 55 | if (GetFunction(f01, 1)) { |
| 56 | queryAddr = f01.GetQueryBase(); |
| 57 | |
| 58 | rc = Read(queryAddr, basicQuery, RMI_DEVICE_F01_BASIC_QUERY_LEN); |
| 59 | if (rc < 0) { |
| 60 | fprintf(stderr, "Failed to read the basic query: %s\n", strerror(errno)); |
| 61 | return rc; |
| 62 | } |
| 63 | m_manufacturerID = basicQuery[0]; |
| 64 | m_hasLTS = basicQuery[1] & RMI_DEVICE_F01_QRY1_HAS_LTS; |
| 65 | m_hasSensorID = basicQuery[1] & RMI_DEVICE_F01_QRY1_HAS_SENSOR_ID; |
| 66 | m_hasAdjustableDoze = basicQuery[1] & RMI_DEVICE_F01_QRY1_HAS_ADJ_DOZE; |
| 67 | m_hasAdjustableDozeHoldoff = basicQuery[1] & RMI_DEVICE_F01_QRY1_HAS_ADJ_DOZE_HOFF; |
| 68 | m_hasQuery42 = basicQuery[1] & RMI_DEVICE_F01_QRY1_HAS_PROPS_2; |
| 69 | m_productInfo = ((basicQuery[2] & RMI_DEVICE_F01_PRODUCTINFO_MASK) << 7) | |
| 70 | (basicQuery[3] & RMI_DEVICE_F01_PRODUCTINFO_MASK); |
| 71 | |
| 72 | snprintf(m_dom, sizeof(m_dom), "20%02d/%02d/%02d", |
| 73 | basicQuery[5] & RMI_DEVICE_F01_QRY5_YEAR_MASK, |
| 74 | basicQuery[6] & RMI_DEVICE_F01_QRY6_MONTH_MASK, |
| 75 | basicQuery[7] & RMI_DEVICE_F01_QRY7_DAY_MASK); |
| 76 | |
| 77 | memcpy(m_productID, &basicQuery[11], RMI_PRODUCT_ID_LENGTH); |
| 78 | m_productID[RMI_PRODUCT_ID_LENGTH] = '\0'; |
| 79 | |
| 80 | queryAddr += 11; |
| 81 | prodInfoAddr = queryAddr + 6; |
| 82 | queryAddr += 10; |
| 83 | |
| 84 | if (m_hasLTS) |
| 85 | ++queryAddr; |
| 86 | |
| 87 | if (m_hasSensorID) { |
| 88 | rc = Read(queryAddr++, &m_sensorID, 1); |
| 89 | if (rc < 0) { |
| 90 | fprintf(stderr, "Failed to read sensor id: %s\n", strerror(errno)); |
| 91 | return rc; |
| 92 | } |
| 93 | } |
| 94 | |
| 95 | if (m_hasLTS) |
| 96 | queryAddr += RMI_DEVICE_F01_LTS_RESERVED_SIZE; |
| 97 | |
| 98 | if (m_hasQuery42) { |
| 99 | rc = Read(queryAddr++, infoBuf, 1); |
| 100 | if (rc < 0) { |
| 101 | fprintf(stderr, "Failed to read query 42: %s\n", strerror(errno)); |
| 102 | return rc; |
| 103 | } |
| 104 | |
| 105 | m_hasDS4Queries = infoBuf[0] & RMI_DEVICE_F01_QRY42_DS4_QUERIES; |
| 106 | m_hasMultiPhysical = infoBuf[0] & RMI_DEVICE_F01_QRY42_MULTI_PHYS; |
| 107 | } |
| 108 | |
| 109 | if (m_hasDS4Queries) { |
| 110 | rc = Read(queryAddr++, &m_ds4QueryLength, 1); |
| 111 | if (rc < 0) { |
| 112 | fprintf(stderr, "Failed to read DS4 query length: %s\n", strerror(errno)); |
| 113 | return rc; |
| 114 | } |
| 115 | } |
| 116 | |
| 117 | for (int i = 1; i <= m_ds4QueryLength; ++i) { |
| 118 | unsigned char val; |
| 119 | rc = Read(queryAddr++, &val, 1); |
| 120 | if (rc < 0) { |
| 121 | fprintf(stderr, "Failed to read F01 Query43.%02d: %s\n", i, strerror(errno)); |
| 122 | continue; |
| 123 | } |
| 124 | |
| 125 | switch(i) { |
| 126 | case 1: |
| 127 | m_hasPackageIDQuery = val & RMI_DEVICE_F01_QRY43_01_PACKAGE_ID; |
| 128 | m_hasBuildIDQuery = val & RMI_DEVICE_F01_QRY43_01_BUILD_ID; |
| 129 | break; |
| 130 | case 2: |
| 131 | case 3: |
| 132 | default: |
| 133 | break; |
| 134 | } |
| 135 | } |
| 136 | |
| 137 | if (m_hasPackageIDQuery) { |
| 138 | rc = Read(prodInfoAddr++, infoBuf, PACKAGE_ID_BYTES); |
| 139 | if (rc > 0) { |
| 140 | unsigned short *val = (unsigned short *)infoBuf; |
| 141 | m_packageID = *val; |
| 142 | val = (unsigned short *)(infoBuf + 2); |
| 143 | m_packageRev = *val; |
| 144 | } |
| 145 | } |
| 146 | |
| 147 | if (m_hasBuildIDQuery) { |
| 148 | rc = Read(prodInfoAddr, infoBuf, BUILD_ID_BYTES); |
| 149 | if (rc > 0) { |
| 150 | unsigned short *val = (unsigned short *)infoBuf; |
| 151 | m_buildID = *val; |
| 152 | m_buildID += infoBuf[2] * 65536; |
| 153 | } |
| 154 | } |
| 155 | } |
| 156 | return 0; |
| 157 | } |
| 158 | |
| 159 | void RMIDevice::PrintProperties() |
| 160 | { |
| 161 | fprintf(stdout, "manufacturerID:\t\t%d\n", m_manufacturerID); |
| 162 | fprintf(stdout, "Has LTS?:\t\t%d\n", m_hasLTS); |
| 163 | fprintf(stdout, "Has Sensor ID?:\t\t%d\n", m_hasSensorID); |
| 164 | fprintf(stdout, "Has Adjustable Doze?:\t%d\n", m_hasAdjustableDoze); |
| 165 | fprintf(stdout, "Has Query 42?:\t\t%d\n", m_hasQuery42); |
| 166 | fprintf(stdout, "Date of Manufacturer:\t%s\n", m_dom); |
| 167 | fprintf(stdout, "Product ID:\t\t%s\n", m_productID); |
| 168 | fprintf(stdout, "Product Info:\t\t%d\n", m_productInfo); |
| 169 | fprintf(stdout, "Package ID:\t\t%d\n", m_packageID); |
| 170 | fprintf(stdout, "Package Rev:\t\t%d\n", m_packageRev); |
| 171 | fprintf(stdout, "Build ID:\t\t%ld\n", m_buildID); |
| 172 | fprintf(stdout, "Sensor ID:\t\t%d\n", m_sensorID); |
| 173 | fprintf(stdout, "Has DS4 Queries?:\t%d\n", m_hasDS4Queries); |
| 174 | fprintf(stdout, "Has Multi Phys?:\t%d\n", m_hasMultiPhysical); |
| 175 | fprintf(stdout, "\n"); |
| 176 | } |
| 177 | |
| 178 | int RMIDevice::Reset() |
| 179 | { |
| 180 | int rc; |
| 181 | RMIFunction f01; |
| 182 | struct timespec ts; |
| 183 | struct timespec rem; |
| 184 | const unsigned char deviceReset = RMI_F01_CMD_DEVICE_RESET; |
| 185 | |
| 186 | if (!GetFunction(f01, 1)) |
| 187 | return -1; |
| 188 | |
| 189 | fprintf(stdout, "Resetting...\n"); |
| 190 | rc = Write(f01.GetCommandBase(), &deviceReset, 1); |
| 191 | if (rc < 0) |
| 192 | return rc; |
| 193 | |
| 194 | ts.tv_sec = RMI_F01_DEFAULT_RESET_DELAY_MS / 1000; |
| 195 | ts.tv_nsec = (RMI_F01_DEFAULT_RESET_DELAY_MS % 1000) * 1000 * 1000; |
| 196 | for (;;) { |
| 197 | if (nanosleep(&ts, &rem) == 0) { |
| 198 | break; |
| 199 | } else { |
| 200 | if (errno == EINTR) { |
| 201 | ts = rem; |
| 202 | continue; |
| 203 | } |
| 204 | return -1; |
| 205 | } |
| 206 | } |
| 207 | fprintf(stdout, "Reset completed.\n"); |
| 208 | return 0; |
| 209 | } |
| 210 | |
| 211 | bool RMIDevice::GetFunction(RMIFunction &func, int functionNumber) |
| 212 | { |
| 213 | std::vector<RMIFunction>::iterator funcIter; |
| 214 | |
| 215 | for (funcIter = m_functionList.begin(); funcIter != m_functionList.end(); ++funcIter) { |
| 216 | if (funcIter->GetFunctionNumber() == functionNumber) { |
| 217 | func = *funcIter; |
| 218 | return true; |
| 219 | } |
| 220 | } |
| 221 | return false; |
| 222 | } |
| 223 | |
| 224 | int RMIDevice::ScanPDT() |
| 225 | { |
| 226 | int rc; |
| 227 | unsigned int page; |
| 228 | unsigned int addr; |
| 229 | unsigned char entry[RMI_DEVICE_PDT_ENTRY_SIZE]; |
| 230 | |
| 231 | m_functionList.clear(); |
| 232 | |
| 233 | for (page = 0; page < RMI_DEVICE_MAX_PAGE; ++page) { |
| 234 | unsigned int page_start = RMI_DEVICE_PAGE_SIZE * page; |
| 235 | unsigned int pdt_start = page_start + RMI_DEVICE_PAGE_SCAN_START; |
| 236 | unsigned int pdt_end = page_start + RMI_DEVICE_PAGE_SCAN_END; |
| 237 | bool found = false; |
| 238 | |
| 239 | SetRMIPage(page); |
| 240 | |
| 241 | for (addr = pdt_start; addr >= pdt_end; addr -= RMI_DEVICE_PDT_ENTRY_SIZE) { |
| 242 | rc = Read(addr, entry, RMI_DEVICE_PDT_ENTRY_SIZE); |
| 243 | if (rc < 0) { |
| 244 | fprintf(stderr, "Failed to read PDT entry at address (0x%04x)\n", addr); |
| 245 | return rc; |
| 246 | } |
| 247 | |
| 248 | RMIFunction func(entry); |
| 249 | if (func.GetFunctionNumber() == 0) |
| 250 | break; |
| 251 | |
| 252 | m_functionList.push_back(func); |
| 253 | found = true; |
| 254 | } |
| 255 | |
| 256 | if (!found) |
| 257 | break; |
| 258 | } |
| 259 | |
| 260 | return 0; |
Andrew Duggan | 65e5553 | 2014-04-04 16:59:54 -0700 | [diff] [blame^] | 261 | } |
| 262 | |
| 263 | long long diff_time(struct timespec *start, struct timespec *end) |
| 264 | { |
| 265 | long long diff; |
| 266 | diff = (end->tv_sec - start->tv_sec) * 1000 * 1000; |
| 267 | diff += (end->tv_nsec - start->tv_nsec) / 1000; |
| 268 | return diff; |
Andrew Duggan | 4e81125 | 2014-04-03 15:17:57 -0700 | [diff] [blame] | 269 | } |