hw-monitor.cpp
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
3 
4 
5 #include "hw-monitor.h"
6 
7 namespace rsimpl
8 {
9  namespace hw_monitor
10  {
11 
12  void fill_usb_buffer(int opCodeNumber, int p1, int p2, int p3, int p4, uint8_t * data, int dataLength, uint8_t * bufferToSend, int & length)
13  {
14  uint16_t preHeaderData = IVCAM_MONITOR_MAGIC_NUMBER;
15 
16  uint8_t * writePtr = bufferToSend;
17  int header_size = 4;
18 
19  int cur_index = 2;
20  *(uint16_t *)(writePtr + cur_index) = preHeaderData;
21  cur_index += sizeof(uint16_t);
22  *(int *)(writePtr + cur_index) = opCodeNumber;
23  cur_index += sizeof(uint32_t);
24  *(int *)(writePtr + cur_index) = p1;
25  cur_index += sizeof(uint32_t);
26  *(int *)(writePtr + cur_index) = p2;
27  cur_index += sizeof(uint32_t);
28  *(int *)(writePtr + cur_index) = p3;
29  cur_index += sizeof(uint32_t);
30  *(int *)(writePtr + cur_index) = p4;
31  cur_index += sizeof(uint32_t);
32 
33  if (dataLength)
34  {
35  memcpy(writePtr + cur_index, data, dataLength);
36  cur_index += dataLength;
37  }
38 
39  length = cur_index;
40  *(uint16_t *)bufferToSend = (uint16_t)(length - header_size); // Length doesn't include header
41  }
42 
43 
44  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)
45  {
46  // write
47  errno = 0;
48 
49  int outXfer;
50 
51  if (!mutex.try_lock_for(std::chrono::milliseconds(IVCAM_MONITOR_MUTEX_TIMEOUT))) throw std::runtime_error("timed_mutex::try_lock_for(...) timed out");
52  std::lock_guard<std::timed_mutex> guard(mutex, std::adopt_lock);
53 
54  bulk_transfer(device, IVCAM_MONITOR_ENDPOINT_OUT, out, (int)outSize, &outXfer, 1000); // timeout in ms
55 
56  std::this_thread::sleep_for(std::chrono::milliseconds(20));
57  // read
58  if (in && inSize)
59  {
61 
62  errno = 0;
63 
64  bulk_transfer(device, IVCAM_MONITOR_ENDPOINT_IN, buf, sizeof(buf), &outXfer, 1000);
65  if (outXfer < (int)sizeof(uint32_t)) throw std::runtime_error("incomplete bulk usb transfer");
66 
67  op = *(uint32_t *)buf;
68  if (outXfer > (int)inSize) throw std::runtime_error("bulk transfer failed - user buffer too small");
69  inSize = outXfer;
70  memcpy(in, buf, inSize);
71  }
72  }
73 
74  void send_hw_monitor_command(uvc::device & device, std::timed_mutex & mutex, hwmon_cmd_details & details)
75  {
76  unsigned char outputBuffer[HW_MONITOR_BUFFER_SIZE];
77 
78  uint32_t op;
79  size_t receivedCmdLen = HW_MONITOR_BUFFER_SIZE;
80 
81  execute_usb_command(device, mutex, (uint8_t*)details.sendCommandData, (size_t)details.sizeOfSendCommandData, op, outputBuffer, receivedCmdLen);
82  details.receivedCommandDataLength = receivedCmdLen;
83 
84  if (details.oneDirection) return;
85 
86  if (details.receivedCommandDataLength < 4) throw std::runtime_error("received incomplete response to usb command");
87 
88  details.receivedCommandDataLength -= 4;
89  memcpy(details.receivedOpcode, outputBuffer, 4);
90 
91  if (details.receivedCommandDataLength > 0)
92  memcpy(details.receivedCommandData, outputBuffer + 4, details.receivedCommandDataLength);
93  }
94 
95  void perform_and_send_monitor_command(uvc::device & device, std::timed_mutex & mutex, hwmon_cmd & newCommand)
96  {
97  uint32_t opCodeXmit = (uint32_t)newCommand.cmd;
98 
99  hwmon_cmd_details details;
100  details.oneDirection = newCommand.oneDirection;
101  details.TimeOut = newCommand.TimeOut;
102 
103  fill_usb_buffer(opCodeXmit,
104  newCommand.Param1,
105  newCommand.Param2,
106  newCommand.Param3,
107  newCommand.Param4,
108  newCommand.data,
109  newCommand.sizeOfSendCommandData,
110  details.sendCommandData,
111  details.sizeOfSendCommandData);
112 
113  send_hw_monitor_command(device, mutex, details);
114 
115  // Error/exit conditions
116  if (newCommand.oneDirection)
117  return;
118 
119  memcpy(newCommand.receivedOpcode, details.receivedOpcode, 4);
120  memcpy(newCommand.receivedCommandData, details.receivedCommandData, details.receivedCommandDataLength);
121  newCommand.receivedCommandDataLength = details.receivedCommandDataLength;
122 
123  // endian?
124  uint32_t opCodeAsUint32 = pack(details.receivedOpcode[3], details.receivedOpcode[2], details.receivedOpcode[1], details.receivedOpcode[0]);
125  if (opCodeAsUint32 != opCodeXmit)
126  {
127  throw std::runtime_error("opcodes do not match");
128  }
129  }
130 
131  void perform_and_send_monitor_command(uvc::device & device, std::timed_mutex & mutex, unsigned char /*handle_id*/, hwmon_cmd & newCommand)
132  {
133  uint32_t opCodeXmit = (uint32_t)newCommand.cmd;
134 
135  hwmon_cmd_details details;
136  details.oneDirection = newCommand.oneDirection;
137  details.TimeOut = newCommand.TimeOut;
138 
139  fill_usb_buffer(opCodeXmit,
140  newCommand.Param1,
141  newCommand.Param2,
142  newCommand.Param3,
143  newCommand.Param4,
144  newCommand.data,
145  newCommand.sizeOfSendCommandData,
146  details.sendCommandData,
147  details.sizeOfSendCommandData);
148 
149  send_hw_monitor_command(device, mutex, details);
150 
151  // Error/exit conditions
152  if (newCommand.oneDirection)
153  return;
154 
155  memcpy(newCommand.receivedOpcode, details.receivedOpcode, 4);
156  memcpy(newCommand.receivedCommandData, details.receivedCommandData, details.receivedCommandDataLength);
157  newCommand.receivedCommandDataLength = details.receivedCommandDataLength;
158 
159  // endian?
160  uint32_t opCodeAsUint32 = pack(details.receivedOpcode[3], details.receivedOpcode[2], details.receivedOpcode[1], details.receivedOpcode[0]);
161  if (opCodeAsUint32 != opCodeXmit)
162  {
163  throw std::runtime_error("opcodes do not match");
164  }
165  }
166  void i2c_write_reg(int command, uvc::device & device, uint16_t slave_address, uint16_t reg, uint32_t value)
167  {
168  hw_monitor::hwmon_cmd cmd(command);
169 
170  cmd.Param1 = slave_address;
171  cmd.Param2 = reg;
172  cmd.Param3 = sizeof(value);
173 
174  memcpy(cmd.data, &value, sizeof(value));
175  cmd.sizeOfSendCommandData = sizeof(value);
176 
177  std::timed_mutex mutex;
178  perform_and_send_monitor_command(device, mutex, cmd);
179 
180  return;
181  }
182 
183  // Read a 32 bit value from the i2c register.
184  void i2c_read_reg(int command, uvc::device & device, uint16_t slave_address, uint16_t reg, uint32_t size, byte* data)
185  {
186  hw_monitor::hwmon_cmd cmd(command);
187 
188  cmd.Param1 = slave_address;
189  cmd.Param2 = reg;
190  cmd.Param3 = size;
191  const int num_retries = 10;
192  std::timed_mutex mutex;
193  int retries = 0;
194  do {
195  try {
197 
198  // validate that the size is of 32 bit (value size).
199  if (cmd.receivedCommandDataLength == size)
200  {
201  memcpy(data, cmd.receivedCommandData, cmd.receivedCommandDataLength);
202  break;
203  }
204  }
205  catch (...)
206  {
207  retries++;
208  std::this_thread::sleep_for(std::chrono::milliseconds(100));
209  if (retries == num_retries)
210  {
211  throw;
212  }
213  }
214 
215  } while (retries < num_retries);
216  return;
217  }
218 
219  void check_eeprom_read_write_status(int IRB_opcode, uvc::device & device)
220  {
221  uint32_t value = 0;
222  i2c_read_reg(IRB_opcode, device, 0x42, 0x70, sizeof(uint32_t), (byte*)&value);
223  if (value & 0x100)
224  {
225  throw std::runtime_error(to_string() << "EEPRom Error" << value);
226  }
227  }
228 
229 
230  void read_from_eeprom(int IRB_opcode, int IWB_opcode, uvc::device & device, unsigned int offset, int size, byte* data)
231  {
232  unsigned int command = offset;
233 
234  //bits[0:12] - Offset In EEprom
235  command &= 0x00001FFF;
236 
237  //bit[13] - Direction 0 = read, 1 = write
238  //Doesn't do anything since it is already 0. Just for redability/consistency.
239  command &= 0xFFFFDFFF;
240 
241  //bit[14:15] - Reserved
242  //Nothing to do
243 
244  //bits[16:23] - Size to read
245  unsigned int lengthR = size;
246  lengthR = lengthR << 16;
247 
248  command |= lengthR;
249 
250  //bit[14:15] - Reserved
251  //Nothing to do
252 
253  //expected = 0x100005
254 
255  uint32_t value = 0;
256  i2c_read_reg(IRB_opcode, device, 0x42, 0x70, sizeof(uint32_t), (byte*)&value); //clean the register
257  i2c_write_reg(IWB_opcode, device, 0x42, 0x0C, command);
258  check_eeprom_read_write_status(IRB_opcode, device);
259  i2c_read_reg(IRB_opcode, device, 0x42, 0xD0, size, data);
260 
261  }
262  void get_raw_data(uint8_t opcode, uvc::device & device, std::timed_mutex & mutex, uint8_t * data, size_t & bytesReturned)
263  {
265 
266  perform_and_send_monitor_command(device, mutex, command);
267  memcpy(data, command.receivedCommandData, HW_MONITOR_BUFFER_SIZE);
268  bytesReturned = command.receivedCommandDataLength;
269  }
270  }
271 }
const uint8_t IVCAM_MONITOR_ENDPOINT_IN
Definition: hw-monitor.h:27
uint32_t pack(uint8_t c0, uint8_t c1, uint8_t c2, uint8_t c3)
Definition: types.h:490
void fill_usb_buffer(int opCodeNumber, int p1, int p2, int p3, int p4, uint8_t *data, int dataLength, uint8_t *bufferToSend, int &length)
Definition: hw-monitor.cpp:12
uint8_t data[HW_MONITOR_BUFFER_SIZE]
Definition: hw-monitor.h:71
void check_eeprom_read_write_status(int IRB_opcode, uvc::device &device)
Definition: hw-monitor.cpp:219
GLenum GLuint GLenum GLsizei const GLchar * buf
Definition: glext.h:2479
Definition: archive.h:12
uint8_t receivedCommandData[HW_MONITOR_BUFFER_SIZE]
Definition: hw-monitor.h:75
const uint16_t IVCAM_MONITOR_MAGIC_NUMBER
Definition: hw-monitor.h:37
uint8_t sendCommandData[HW_MONITOR_COMMAND_SIZE]
Definition: hw-monitor.h:85
void perform_and_send_monitor_command(uvc::device &device, std::timed_mutex &mutex, hwmon_cmd &newCommand)
Definition: hw-monitor.cpp:95
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)
Definition: hw-monitor.cpp:44
GLint GLenum GLsizei GLsizei GLsizei GLint GLsizei const void * data
Definition: glext.h:223
GLintptr offset
Definition: glext.h:533
command
Definition: ds-private.cpp:10
void read_from_eeprom(int IRB_opcode, int IWB_opcode, uvc::device &device, unsigned int offset, int size, byte *data)
Definition: hw-monitor.cpp:230
const uint16_t IVCAM_MONITOR_MAX_BUFFER_SIZE
Definition: hw-monitor.h:38
GLsizei const GLfloat * value
Definition: glext.h:693
uint8_t receivedCommandData[HW_MONITOR_BUFFER_SIZE]
Definition: hw-monitor.h:89
const uint16_t HW_MONITOR_BUFFER_SIZE
Definition: hw-monitor.h:41
void bulk_transfer(device &device, unsigned char endpoint, void *data, int length, int *actual_length, unsigned int timeout)
void i2c_write_reg(int command, uvc::device &device, uint16_t slave_address, uint16_t reg, uint32_t value)
Definition: hw-monitor.cpp:166
uint8_t byte
Definition: types.h:42
GLuint in
Definition: glext.h:8313
GLsizeiptr size
Definition: glext.h:532
GLuint GLsizei GLsizei * length
Definition: glext.h:664
void i2c_read_reg(int command, uvc::device &device, uint16_t slave_address, uint16_t reg, uint32_t size, byte *data)
Definition: hw-monitor.cpp:184
void send_hw_monitor_command(uvc::device &device, std::timed_mutex &mutex, hwmon_cmd_details &details)
Definition: hw-monitor.cpp:74
void get_raw_data(uint8_t opcode, uvc::device &device, std::timed_mutex &mutex, uint8_t *data, size_t &bytesReturned)
Definition: hw-monitor.cpp:262
const uint8_t IVCAM_MONITOR_ENDPOINT_OUT
Definition: hw-monitor.h:26
const uint16_t IVCAM_MONITOR_MUTEX_TIMEOUT
Definition: hw-monitor.h:39


librealsense
Author(s): Sergey Dorodnicov , Mark Horn , Reagan Lopez
autogenerated on Fri Mar 13 2020 03:16:17