hw-monitor.cpp
Go to the documentation of this file.
00001 // License: Apache 2.0. See LICENSE file in root directory.
00002 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
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); // Length doesn't include header
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             // write
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); // timeout in ms
00055 
00056             std::this_thread::sleep_for(std::chrono::milliseconds(20));
00057             // read
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             // Error/exit conditions
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             // endian?
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 /*handle_id*/, 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             // Error/exit conditions
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             // endian?
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         // Read a 32 bit value from the i2c register.
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                     // validate that the size is of 32 bit (value size).
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             //bits[0:12] - Offset In EEprom
00235             command &= 0x00001FFF;
00236 
00237             //bit[13] - Direction 0 = read, 1 = write
00238             //Doesn't do anything since it is already 0. Just for redability/consistency.
00239             command &= 0xFFFFDFFF;
00240 
00241             //bit[14:15] - Reserved
00242             //Nothing to do
00243 
00244             //bits[16:23] - Size to read
00245             unsigned int  lengthR = size;
00246             lengthR = lengthR << 16;
00247 
00248             command |= lengthR;
00249 
00250             //bit[14:15] - Reserved
00251             //Nothing to do
00252 
00253             //expected = 0x100005
00254 
00255             uint32_t value = 0;
00256             i2c_read_reg(IRB_opcode, device, 0x42, 0x70, sizeof(uint32_t), (byte*)&value); //clean the register
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 }


librealsense
Author(s): Sergey Dorodnicov , Mark Horn , Reagan Lopez
autogenerated on Tue Jun 25 2019 19:54:39