Line data Source code
1 : #include "SIM_I2CDevice.h"
2 : #include <AP_HAL/utility/sparse-endian.h>
3 :
4 : #ifndef HAL_DEBUG_I2DEVICE
5 : #define HAL_DEBUG_I2DEVICE 0
6 : #endif
7 :
8 : #if HAL_DEBUG_I2DEVICE
9 : #include <GCS_MAVLink/GCS.h>
10 : #define DEBUG(fmt, args ...) do { if (get_debug()) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, fmt, ## args); } } while (0)
11 : #else
12 : #define DEBUG(fmt, args ...)
13 : #endif
14 :
15 288765 : void SITL::I2CRegisters::add_register(const char *name, uint8_t reg, RegMode mode)
16 : {
17 : DEBUG("Adding register %u (0x%02x) (%s)", reg, reg, name);
18 288765 : regname[reg] = name;
19 288765 : if (mode == RegMode::RDONLY || mode == RegMode::RDWR) {
20 215475 : readable_registers.set((uint8_t)reg);
21 : }
22 288765 : if (mode == RegMode::WRONLY || mode == RegMode::RDWR) {
23 123546 : writable_registers.set((uint8_t)reg);
24 : }
25 288765 : }
26 :
27 3758438810 : void SITL::I2CRegisters_16Bit::set_register(uint8_t reg, uint16_t value)
28 : {
29 3758438810 : if (regname[reg] == nullptr) {
30 0 : AP_HAL::panic("Setting un-named register %u", reg);
31 : }
32 : DEBUG("Setting %u (0x%02x) (%s) to 0x%02x", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value);
33 :
34 3758438810 : word[reg] = htobe16(value);
35 3758438810 : }
36 :
37 4574081 : void SITL::I2CRegisters_16Bit::set_register(uint8_t reg, int16_t value)
38 : {
39 4574081 : if (regname[reg] == nullptr) {
40 0 : AP_HAL::panic("Setting un-named register %u", reg);
41 : }
42 : DEBUG("Setting %s (%u) to 0x%02x", regname[reg], (unsigned)reg, (signed)value);
43 4574081 : word[reg] = htobe16(value);
44 4574081 : }
45 :
46 7686 : int SITL::I2CRegisters_16Bit::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
47 : {
48 7686 : if (data->nmsgs == 2) {
49 : // data read request
50 7685 : if (data->msgs[0].flags != 0) {
51 0 : AP_HAL::panic("Unexpected flags");
52 : }
53 7685 : if (data->msgs[1].flags != I2C_M_RD) {
54 0 : AP_HAL::panic("Unexpected flags");
55 : }
56 7685 : const uint8_t reg_base_addr = data->msgs[0].buf[0];
57 7685 : uint8_t bytes_copied = 0;
58 14985 : while (bytes_copied < data->msgs[1].len) {
59 7697 : const uint8_t reg_addr = reg_base_addr + bytes_copied/2;
60 7697 : if (!readable_registers.get(reg_addr)) {
61 : // ::printf("Register 0x%02x is not readable!\n", reg_addr);
62 397 : return -1;
63 : }
64 7300 : const uint16_t register_value = word[reg_addr];
65 7300 : data->msgs[1].buf[bytes_copied++] = register_value >> 8;
66 7300 : if (bytes_copied < data->msgs[1].len) {
67 7300 : data->msgs[1].buf[bytes_copied++] = register_value & 0xFF;
68 : }
69 : }
70 7288 : data->msgs[1].len = bytes_copied;
71 7288 : return 0;
72 : }
73 :
74 1 : if (data->nmsgs == 1) {
75 : // data write request
76 1 : if (data->msgs[0].flags != 0) {
77 0 : AP_HAL::panic("Unexpected flags");
78 : }
79 : // FIXME: handle multi-register writes
80 1 : const uint8_t reg_addr = data->msgs[0].buf[0];
81 1 : if (!writable_registers.get(reg_addr)) {
82 0 : AP_HAL::panic("Register 0x%02x is not writable!", reg_addr);
83 : }
84 1 : const uint16_t register_value = data->msgs[0].buf[2] << 8 | data->msgs[0].buf[1];
85 1 : word[reg_addr] = register_value;
86 1 : return 0;
87 : }
88 :
89 0 : return -1;
90 : };
91 :
92 :
93 :
94 357608651 : void SITL::I2CRegisters_8Bit::set_register(uint8_t reg, uint8_t value)
95 : {
96 357608651 : if (regname[reg] == nullptr) {
97 0 : AP_HAL::panic("Setting un-named register %u", reg);
98 : }
99 : DEBUG("Setting %u (0x%02x) (%s) to 0x%02x (%c)", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value, value);
100 357608651 : byte[reg] = value;
101 357608651 : }
102 :
103 0 : void SITL::I2CRegisters_8Bit::set_register(uint8_t reg, int8_t value)
104 : {
105 0 : if (regname[reg] == nullptr) {
106 0 : AP_HAL::panic("Setting un-named register %u", reg);
107 : }
108 : DEBUG("Setting %s (%u) to 0x%02x (%c)", regname[reg], (unsigned)reg, (signed)value, value);
109 0 : byte[reg] = value;
110 0 : }
111 :
112 2356563 : int SITL::I2CRegisters_8Bit::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
113 : {
114 2356563 : if (data->nmsgs == 2) {
115 : // data read request
116 10471 : if (data->msgs[0].flags != 0) {
117 0 : AP_HAL::panic("Unexpected flags");
118 : }
119 10471 : if (data->msgs[1].flags != I2C_M_RD) {
120 0 : AP_HAL::panic("Unexpected flags");
121 : }
122 10471 : const uint8_t reg_base_addr = data->msgs[0].buf[0];
123 10471 : uint8_t bytes_copied = 0;
124 20942 : while (bytes_copied < data->msgs[1].len) {
125 10471 : const uint8_t reg_addr = reg_base_addr + bytes_copied;
126 10471 : if (!readable_registers.get(reg_addr)) {
127 : // ::printf("Register 0x%02x is not readable!\n", reg_addr);
128 0 : return -1;
129 : }
130 10471 : const uint8_t register_value = byte[reg_addr];
131 10471 : data->msgs[1].buf[bytes_copied++] = register_value;
132 : }
133 10471 : data->msgs[1].len = bytes_copied;
134 10471 : return 0;
135 : }
136 :
137 2346092 : if (data->nmsgs == 1) {
138 : // data write request
139 2346092 : if (data->msgs[0].flags != 0) {
140 0 : AP_HAL::panic("Unexpected flags");
141 : }
142 2346092 : const uint8_t reg_base_addr = data->msgs[0].buf[0];
143 2346092 : uint8_t bytes_copied = 0;
144 5424334 : while (bytes_copied < data->msgs[0].len-1) {
145 3078242 : const uint8_t reg_addr = reg_base_addr + bytes_copied;
146 3078242 : if (!writable_registers.get(reg_addr)) {
147 0 : AP_HAL::panic("Register 0x%02x is not writable!", reg_addr);
148 : }
149 3078242 : const uint8_t register_value = data->msgs[0].buf[1+bytes_copied];
150 3078242 : rdwr_store_register_value(reg_addr, register_value);
151 3078242 : bytes_copied++;
152 : }
153 2346092 : return 0;
154 : }
155 :
156 0 : return -1;
157 : };
158 :
159 :
160 0 : void SITL::I2CRegisters_8Bit::assert_register_value(uint8_t reg, uint8_t value)
161 : {
162 0 : if (byte[reg] != value) {
163 0 : AP_HAL::panic("Register 0x%02x (%s) was expected to have value (%02x) but has value (%02x)", reg, regname[reg], byte[reg], value);
164 : }
165 0 : }
166 :
167 9012 : int SITL::I2CCommandResponseDevice::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
168 : {
169 9012 : const uint32_t now = AP_HAL::millis();
170 :
171 9012 : struct I2C::i2c_msg &msg = data->msgs[0];
172 9012 : if (msg.flags == I2C_M_RD) {
173 : // driver is attempting to receive reading...
174 4502 : if (now - cmd_take_reading_received_ms < command_processing_time_ms()) {
175 : // not sure we ought to be returning -1 here - what does
176 : // the real device do? return stale data? garbage data?
177 0 : return -1;
178 : }
179 4502 : if (msg.len != 2) {
180 0 : AP_HAL::panic("Unxpected message length (%u)", msg.len);
181 : }
182 :
183 4502 : const uint16_t value = reading();
184 :
185 4502 : msg.buf[0] = value >> 8;
186 4502 : msg.buf[1] = value & 0xff;
187 :
188 4502 : return 0;
189 : }
190 :
191 4510 : const uint8_t cmd = msg.buf[0];
192 4510 : if (cmd != command_take_reading()) {
193 0 : AP_HAL::panic("Unknown command (%u)", cmd);
194 : }
195 4510 : cmd_take_reading_received_ms = now;
196 :
197 4510 : return 0;
198 : }
199 :
200 :
201 8376 : void SITL::I2CRegisters_ConfigurableLength::add_register(const char *name, uint8_t reg, uint8_t len, RegMode mode)
202 : {
203 8376 : SITL::I2CRegisters::add_register(name, reg, mode);
204 8376 : if (len > 4) {
205 0 : AP_HAL::panic("Only up to 4 bytes");
206 : }
207 8376 : reg_data_len[reg] = len;
208 8376 : }
209 :
210 0 : void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, uint16_t value)
211 : {
212 0 : if (regname[reg] == nullptr) {
213 0 : AP_HAL::panic("Setting un-named register %u", reg);
214 : }
215 : DEBUG("Setting %u (0x%02x) (%s) to 0x%02x", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value);
216 :
217 0 : if (reg_data_len[reg] != 2) {
218 0 : AP_HAL::panic("Invalid set_register len");
219 : }
220 0 : reg_data[reg] = htobe16(value);
221 0 : }
222 :
223 0 : void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, int16_t value)
224 : {
225 0 : if (regname[reg] == nullptr) {
226 0 : AP_HAL::panic("Setting un-named register %u", reg);
227 : }
228 : DEBUG("Setting %s (%u) to 0x%02x", regname[reg], (unsigned)reg, (signed)value);
229 :
230 0 : if (reg_data_len[reg] != 2) {
231 0 : AP_HAL::panic("Invalid set_register len");
232 : }
233 0 : reg_data[reg] = htobe16(value);
234 0 : }
235 :
236 6282 : void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, uint8_t value)
237 : {
238 6282 : if (regname[reg] == nullptr) {
239 0 : AP_HAL::panic("Setting un-named register %u", reg);
240 : }
241 : DEBUG("Setting %u (0x%02x) (%s) to 0x%02x (%c)", (unsigned)reg, (unsigned)reg, regname[reg], (unsigned)value, value);
242 6282 : if (reg_data_len[reg] != 1) {
243 0 : AP_HAL::panic("Invalid set_register len");
244 : }
245 6282 : reg_data[reg] = value << 24;
246 6282 : }
247 :
248 0 : void SITL::I2CRegisters_ConfigurableLength::set_register(uint8_t reg, int8_t value)
249 : {
250 0 : if (regname[reg] == nullptr) {
251 0 : AP_HAL::panic("Setting un-named register %u", reg);
252 : }
253 : DEBUG("Setting %s (%u) to 0x%02x (%c)", regname[reg], (unsigned)reg, (signed)value, value);
254 0 : if (reg_data_len[reg] != 1) {
255 0 : AP_HAL::panic("Invalid set_register len");
256 : }
257 0 : reg_data[reg] = value << 24;
258 0 : }
259 :
260 0 : int SITL::I2CRegisters_ConfigurableLength::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
261 : {
262 0 : if (data->nmsgs == 2) {
263 : // data read request
264 0 : if (data->msgs[0].flags != 0) {
265 0 : AP_HAL::panic("Unexpected flags");
266 : }
267 0 : if (data->msgs[1].flags != I2C_M_RD) {
268 0 : AP_HAL::panic("Unexpected flags");
269 : }
270 0 : const uint8_t reg_addr = data->msgs[0].buf[0];
271 0 : if (data->msgs[1].len != reg_data_len[reg_addr]) {
272 0 : AP_HAL::panic("Invalid rdwr len");
273 : }
274 0 : if (!readable_registers.get(reg_addr)) {
275 : // ::printf("Register 0x%02x is not readable!\n", reg_addr);
276 0 : return -1;
277 : }
278 0 : const uint32_t register_value = reg_data[reg_addr];
279 0 : if (data->msgs[1].len == 1) {
280 0 : data->msgs[1].buf[0] = register_value >> 24;
281 0 : } else if (data->msgs[1].len == 2) {
282 0 : const uint16_t v = htobe16(register_value & 0xffff);
283 0 : memcpy(&(data->msgs[1].buf[0]), &v, 2);
284 : } else {
285 0 : AP_HAL::panic("Bad length"); // FIXME
286 : }
287 0 : data->msgs[1].len = reg_data_len[reg_addr];
288 0 : return 0;
289 : }
290 :
291 0 : if (data->nmsgs == 1) {
292 : // data write request
293 0 : if (data->msgs[0].flags != 0) {
294 0 : AP_HAL::panic("Unexpected flags");
295 : }
296 : // FIXME: handle multi-register writes
297 0 : const uint8_t reg_addr = data->msgs[0].buf[0];
298 0 : if (!writable_registers.get(reg_addr)) {
299 0 : AP_HAL::panic("Register 0x%02x is not writable!", reg_addr);
300 : }
301 0 : const uint8_t data_msg_len = data->msgs[0].len - 1;
302 0 : if (data_msg_len != reg_data_len[reg_addr]) {
303 0 : AP_HAL::panic("Invalid rdwr len");
304 : }
305 0 : memcpy((uint8_t*)®_data[reg_addr], &data->msgs[0].buf[1], data_msg_len);
306 0 : return 0;
307 : }
308 :
309 0 : return -1;
310 : };
311 :
312 1488167 : void SITL::I2CRegisters_ConfigurableLength::get_reg_value(uint8_t reg, uint8_t &value) const
313 : {
314 1488167 : if (reg_data_len[reg] != 1) {
315 0 : AP_HAL::panic("Invalid reg_reg_value len");
316 : }
317 1488167 : value = reg_data[reg];
318 1488167 : }
|
