00001
00002
00003
00004
00005 #include "hw-monitor.h"
00006
00007 namespace rsimpl
00008 {
00009 namespace hw_monitor
00010 {
00011
00012 void fill_usb_buffer(int opCodeNumber, int p1, int p2, int p3, int p4, uint8_t * data, int dataLength, uint8_t * bufferToSend, int & length)
00013 {
00014 uint16_t preHeaderData = IVCAM_MONITOR_MAGIC_NUMBER;
00015
00016 uint8_t * writePtr = bufferToSend;
00017 int header_size = 4;
00018
00019 int cur_index = 2;
00020 *(uint16_t *)(writePtr + cur_index) = preHeaderData;
00021 cur_index += sizeof(uint16_t);
00022 *(int *)(writePtr + cur_index) = opCodeNumber;
00023 cur_index += sizeof(uint32_t);
00024 *(int *)(writePtr + cur_index) = p1;
00025 cur_index += sizeof(uint32_t);
00026 *(int *)(writePtr + cur_index) = p2;
00027 cur_index += sizeof(uint32_t);
00028 *(int *)(writePtr + cur_index) = p3;
00029 cur_index += sizeof(uint32_t);
00030 *(int *)(writePtr + cur_index) = p4;
00031 cur_index += sizeof(uint32_t);
00032
00033 if (dataLength)
00034 {
00035 memcpy(writePtr + cur_index, data, dataLength);
00036 cur_index += dataLength;
00037 }
00038
00039 length = cur_index;
00040 *(uint16_t *)bufferToSend = (uint16_t)(length - header_size);
00041 }
00042
00043
00044 void execute_usb_command(uvc::device & device, std::timed_mutex & mutex, uint8_t *out, size_t outSize, uint32_t & op, uint8_t * in, size_t & inSize)
00045 {
00046
00047 errno = 0;
00048
00049 int outXfer;
00050
00051 if (!mutex.try_lock_for(std::chrono::milliseconds(IVCAM_MONITOR_MUTEX_TIMEOUT))) throw std::runtime_error("timed_mutex::try_lock_for(...) timed out");
00052 std::lock_guard<std::timed_mutex> guard(mutex, std::adopt_lock);
00053
00054 bulk_transfer(device, IVCAM_MONITOR_ENDPOINT_OUT, out, (int)outSize, &outXfer, 1000);
00055
00056 std::this_thread::sleep_for(std::chrono::milliseconds(20));
00057
00058 if (in && inSize)
00059 {
00060 uint8_t buf[IVCAM_MONITOR_MAX_BUFFER_SIZE];
00061
00062 errno = 0;
00063
00064 bulk_transfer(device, IVCAM_MONITOR_ENDPOINT_IN, buf, sizeof(buf), &outXfer, 1000);
00065 if (outXfer < (int)sizeof(uint32_t)) throw std::runtime_error("incomplete bulk usb transfer");
00066
00067 op = *(uint32_t *)buf;
00068 if (outXfer > (int)inSize) throw std::runtime_error("bulk transfer failed - user buffer too small");
00069 inSize = outXfer;
00070 memcpy(in, buf, inSize);
00071 }
00072 }
00073
00074 void send_hw_monitor_command(uvc::device & device, std::timed_mutex & mutex, hwmon_cmd_details & details)
00075 {
00076 unsigned char outputBuffer[HW_MONITOR_BUFFER_SIZE];
00077
00078 uint32_t op;
00079 size_t receivedCmdLen = HW_MONITOR_BUFFER_SIZE;
00080
00081 execute_usb_command(device, mutex, (uint8_t*)details.sendCommandData, (size_t)details.sizeOfSendCommandData, op, outputBuffer, receivedCmdLen);
00082 details.receivedCommandDataLength = receivedCmdLen;
00083
00084 if (details.oneDirection) return;
00085
00086 if (details.receivedCommandDataLength < 4) throw std::runtime_error("received incomplete response to usb command");
00087
00088 details.receivedCommandDataLength -= 4;
00089 memcpy(details.receivedOpcode, outputBuffer, 4);
00090
00091 if (details.receivedCommandDataLength > 0)
00092 memcpy(details.receivedCommandData, outputBuffer + 4, details.receivedCommandDataLength);
00093 }
00094
00095 void perform_and_send_monitor_command(uvc::device & device, std::timed_mutex & mutex, hwmon_cmd & newCommand)
00096 {
00097 uint32_t opCodeXmit = (uint32_t)newCommand.cmd;
00098
00099 hwmon_cmd_details details;
00100 details.oneDirection = newCommand.oneDirection;
00101 details.TimeOut = newCommand.TimeOut;
00102
00103 fill_usb_buffer(opCodeXmit,
00104 newCommand.Param1,
00105 newCommand.Param2,
00106 newCommand.Param3,
00107 newCommand.Param4,
00108 newCommand.data,
00109 newCommand.sizeOfSendCommandData,
00110 details.sendCommandData,
00111 details.sizeOfSendCommandData);
00112
00113 send_hw_monitor_command(device, mutex, details);
00114
00115
00116 if (newCommand.oneDirection)
00117 return;
00118
00119 memcpy(newCommand.receivedOpcode, details.receivedOpcode, 4);
00120 memcpy(newCommand.receivedCommandData, details.receivedCommandData, details.receivedCommandDataLength);
00121 newCommand.receivedCommandDataLength = details.receivedCommandDataLength;
00122
00123
00124 uint32_t opCodeAsUint32 = pack(details.receivedOpcode[3], details.receivedOpcode[2], details.receivedOpcode[1], details.receivedOpcode[0]);
00125 if (opCodeAsUint32 != opCodeXmit)
00126 {
00127 throw std::runtime_error("opcodes do not match");
00128 }
00129 }
00130
00131 void perform_and_send_monitor_command(uvc::device & device, std::timed_mutex & mutex, unsigned char , hwmon_cmd & newCommand)
00132 {
00133 uint32_t opCodeXmit = (uint32_t)newCommand.cmd;
00134
00135 hwmon_cmd_details details;
00136 details.oneDirection = newCommand.oneDirection;
00137 details.TimeOut = newCommand.TimeOut;
00138
00139 fill_usb_buffer(opCodeXmit,
00140 newCommand.Param1,
00141 newCommand.Param2,
00142 newCommand.Param3,
00143 newCommand.Param4,
00144 newCommand.data,
00145 newCommand.sizeOfSendCommandData,
00146 details.sendCommandData,
00147 details.sizeOfSendCommandData);
00148
00149 send_hw_monitor_command(device, mutex, details);
00150
00151
00152 if (newCommand.oneDirection)
00153 return;
00154
00155 memcpy(newCommand.receivedOpcode, details.receivedOpcode, 4);
00156 memcpy(newCommand.receivedCommandData, details.receivedCommandData, details.receivedCommandDataLength);
00157 newCommand.receivedCommandDataLength = details.receivedCommandDataLength;
00158
00159
00160 uint32_t opCodeAsUint32 = pack(details.receivedOpcode[3], details.receivedOpcode[2], details.receivedOpcode[1], details.receivedOpcode[0]);
00161 if (opCodeAsUint32 != opCodeXmit)
00162 {
00163 throw std::runtime_error("opcodes do not match");
00164 }
00165 }
00166 void i2c_write_reg(int command, uvc::device & device, uint16_t slave_address, uint16_t reg, uint32_t value)
00167 {
00168 hw_monitor::hwmon_cmd cmd(command);
00169
00170 cmd.Param1 = slave_address;
00171 cmd.Param2 = reg;
00172 cmd.Param3 = sizeof(value);
00173
00174 memcpy(cmd.data, &value, sizeof(value));
00175 cmd.sizeOfSendCommandData = sizeof(value);
00176
00177 std::timed_mutex mutex;
00178 perform_and_send_monitor_command(device, mutex, cmd);
00179
00180 return;
00181 }
00182
00183
00184 void i2c_read_reg(int command, uvc::device & device, uint16_t slave_address, uint16_t reg, uint32_t size, byte* data)
00185 {
00186 hw_monitor::hwmon_cmd cmd(command);
00187
00188 cmd.Param1 = slave_address;
00189 cmd.Param2 = reg;
00190 cmd.Param3 = size;
00191 const int num_retries = 10;
00192 std::timed_mutex mutex;
00193 int retries = 0;
00194 do {
00195 try {
00196 hw_monitor::perform_and_send_monitor_command(device, mutex, cmd);
00197
00198
00199 if (cmd.receivedCommandDataLength == size)
00200 {
00201 memcpy(data, cmd.receivedCommandData, cmd.receivedCommandDataLength);
00202 break;
00203 }
00204 }
00205 catch (...)
00206 {
00207 retries++;
00208 std::this_thread::sleep_for(std::chrono::milliseconds(100));
00209 if (retries == num_retries)
00210 {
00211 throw;
00212 }
00213 }
00214
00215 } while (retries < num_retries);
00216 return;
00217 }
00218
00219 void check_eeprom_read_write_status(int IRB_opcode, uvc::device & device)
00220 {
00221 uint32_t value = 0;
00222 i2c_read_reg(IRB_opcode, device, 0x42, 0x70, sizeof(uint32_t), (byte*)&value);
00223 if (value & 0x100)
00224 {
00225 throw std::runtime_error(to_string() << "EEPRom Error" << value);
00226 }
00227 }
00228
00229
00230 void read_from_eeprom(int IRB_opcode, int IWB_opcode, uvc::device & device, unsigned int offset, int size, byte* data)
00231 {
00232 unsigned int command = offset;
00233
00234
00235 command &= 0x00001FFF;
00236
00237
00238
00239 command &= 0xFFFFDFFF;
00240
00241
00242
00243
00244
00245 unsigned int lengthR = size;
00246 lengthR = lengthR << 16;
00247
00248 command |= lengthR;
00249
00250
00251
00252
00253
00254
00255 uint32_t value = 0;
00256 i2c_read_reg(IRB_opcode, device, 0x42, 0x70, sizeof(uint32_t), (byte*)&value);
00257 i2c_write_reg(IWB_opcode, device, 0x42, 0x0C, command);
00258 check_eeprom_read_write_status(IRB_opcode, device);
00259 i2c_read_reg(IRB_opcode, device, 0x42, 0xD0, size, data);
00260
00261 }
00262 void get_raw_data(uint8_t opcode, uvc::device & device, std::timed_mutex & mutex, uint8_t * data, size_t & bytesReturned)
00263 {
00264 hw_monitor::hwmon_cmd command(opcode);
00265
00266 perform_and_send_monitor_command(device, mutex, command);
00267 memcpy(data, command.receivedCommandData, HW_MONITOR_BUFFER_SIZE);
00268 bytesReturned = command.receivedCommandDataLength;
00269 }
00270 }
00271 }