1 /* $NetBSD: common.c,v 1.1 2021/12/07 17:39:55 brad Exp $ */
2
3 /*
4 * Copyright (c) 2021 Brad Spencer <brad@anduin.eldar.org>
5 *
6 * Permission to use, copy, modify, and distribute this software for any
7 * purpose with or without fee is hereby granted, provided that the above
8 * copyright notice and this permission notice appear in all copies.
9 *
10 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
11 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
12 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
13 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
14 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
15 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
16 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
17 */
18
19 #ifdef __RCSID
20 __RCSID("$NetBSD: common.c,v 1.1 2021/12/07 17:39:55 brad Exp $");
21 #endif
22
23 /* Common functions dealing with the SCMD devices. This does not
24 * know how to talk to anything in particular, it calls out to the
25 * functions defined in the function blocks for that.
26 */
27
28 #include <inttypes.h>
29 #include <stdbool.h>
30 #include <stdio.h>
31 #include <stdlib.h>
32 #include <unistd.h>
33 #include <err.h>
34 #include <fcntl.h>
35 #include <string.h>
36 #include <limits.h>
37 #include <errno.h>
38
39 #include <dev/ic/scmdreg.h>
40
41 #define EXTERN
42 #include "common.h"
43 #include "responses.h"
44 #include "scmdctl.h"
45
46
47 int
decode_motor_level(int raw)48 decode_motor_level(int raw)
49 {
50 int r;
51
52 r = abs(128 - raw);
53 if (raw < 128)
54 r = r * -1;
55
56 return r;
57 }
58
common_clear(struct function_block * fb,int fd,bool debug)59 int common_clear(struct function_block *fb, int fd, bool debug)
60 {
61 return (*(fb->func_clear))(fd, debug);
62 }
63
64 int
common_identify(struct function_block * fb,int fd,bool debug,int a_module,struct scmd_identify_response * r)65 common_identify(struct function_block *fb, int fd, bool debug, int a_module, struct scmd_identify_response *r)
66 {
67 uint8_t b;
68 int err;
69
70 err = (*(fb->func_clear))(fd, debug);
71 if (! err) {
72 err = (*(fb->func_phy_read))(fd, debug, a_module, SCMD_REG_ID, SCMD_REG_ID, &b);
73 if (! err)
74 r->id = b;
75 err = (*(fb->func_phy_read))(fd, debug, a_module, SCMD_REG_FID, SCMD_REG_FID, &b);
76 if (! err)
77 r->fwversion = b;
78 err = (*(fb->func_phy_read))(fd, debug, a_module, SCMD_REG_CONFIG_BITS, SCMD_REG_CONFIG_BITS, &b);
79 if (! err)
80 r->config_bits = b;
81 err = (*(fb->func_phy_read))(fd, debug, a_module, SCMD_REG_SLAVE_ADDR, SCMD_REG_SLAVE_ADDR, &b);
82 if (! err)
83 r->slv_i2c_address = b;
84 }
85
86 return err;
87 }
88
common_diag(struct function_block * fb,int fd,bool debug,int a_module,struct scmd_diag_response * r)89 int common_diag(struct function_block *fb, int fd, bool debug, int a_module, struct scmd_diag_response *r)
90 {
91 uint8_t b;
92 int err, m;
93
94 err = (*(fb->func_clear))(fd, debug);
95 if (! err) {
96 m = 0;
97 for(uint8_t n = SCMD_REG_U_I2C_RD_ERR; n <= SCMD_REG_GEN_TEST_WORD; n++) {
98 err = (*(fb->func_phy_read))(fd, debug, a_module, n, n, &b);
99 if (! err) {
100 r->diags[m] = b;
101 }
102 m++;
103 }
104 }
105
106 return err;
107 }
108
109 /* This tries to avoid reading just every register if only one
110 * motor is asked about.
111 */
112 int
common_get_motor(struct function_block * fb,int fd,bool debug,int a_module,struct scmd_motor_response * r)113 common_get_motor(struct function_block *fb, int fd, bool debug, int a_module, struct scmd_motor_response *r)
114 {
115 uint8_t b;
116 int err = 0,m;
117
118 if (a_module != SCMD_ANY_MODULE &&
119 (a_module < 0 || a_module > 16))
120 return EINVAL;
121
122 err = (*(fb->func_clear))(fd, debug);
123 if (! err) {
124 err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_DRIVER_ENABLE, SCMD_REG_DRIVER_ENABLE, &b);
125 if (! err)
126 r->driver = b;
127
128 m = 0;
129 for(uint8_t n = SCMD_REG_MA_DRIVE; n <= SCMD_REG_S16B_DRIVE; n++) {
130 r->motorlevels[m] = SCMD_NO_MOTOR;
131 if (a_module != SCMD_ANY_MODULE &&
132 (m / 2) != a_module)
133 goto skip;
134 err = (*(fb->func_phy_read))(fd, debug, 0, n, n, &b);
135 if (! err)
136 r->motorlevels[m] = b;
137 skip:
138 m++;
139 }
140
141 if (a_module == SCMD_ANY_MODULE ||
142 a_module == 0) {
143 err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_MOTOR_A_INVERT, SCMD_REG_MOTOR_A_INVERT, &b);
144 if (!err)
145 r->invert[0] = (b & 0x01);
146 err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_MOTOR_B_INVERT, SCMD_REG_MOTOR_B_INVERT, &b);
147 if (!err)
148 r->invert[1] = (b & 0x01);
149 }
150
151 if (a_module != 0) {
152 m = 2;
153 for(uint8_t n = SCMD_REG_INV_2_9; n <= SCMD_REG_INV_26_33; n++) {
154 err = (*(fb->func_phy_read))(fd, debug, 0, n, n, &b);
155 if (!err) {
156 for(uint8_t j = 0; j < 8;j++) {
157 r->invert[m] = (b & (1 << j));
158 m++;
159 }
160 }
161 }
162 }
163
164 if (a_module == SCMD_ANY_MODULE ||
165 a_module == 0) {
166 err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_BRIDGE, SCMD_REG_BRIDGE, &b);
167 if (!err)
168 r->bridge[0] = (b & 0x01);
169 }
170
171 if (a_module != 0) {
172 m = 1;
173 for(uint8_t n = SCMD_REG_BRIDGE_SLV_L; n <= SCMD_REG_BRIDGE_SLV_H; n++) {
174 err = (*(fb->func_phy_read))(fd, debug, 0, n, n, &b);
175 if (!err) {
176 for(uint8_t j = 0; j < 8;j++) {
177 r->bridge[m] = (b & (1 << j));
178 m++;
179 }
180 }
181 }
182 }
183 }
184
185 return err;
186 }
187
188 int
common_set_motor(struct function_block * fb,int fd,bool debug,int a_module,char a_motor,int8_t reg_v)189 common_set_motor(struct function_block *fb, int fd, bool debug, int a_module, char a_motor, int8_t reg_v)
190 {
191 uint8_t reg;
192 int err;
193 int reg_index;
194
195 if (a_module < 0 || a_module > 16)
196 return EINVAL;
197
198 if (!(a_motor == 'A' || a_motor == 'B'))
199 return EINVAL;
200
201 err = (*(fb->func_clear))(fd, debug);
202 if (! err) {
203 reg_index = a_module * 2;
204 if (a_motor == 'B')
205 reg_index++;
206 reg = SCMD_REG_MA_DRIVE + reg_index;
207 reg_v = reg_v + 128;
208 if (debug)
209 fprintf(stderr,"common_set_motor: reg_index: %d ; reg: %02X ; reg_v: %d\n",reg_index,reg,reg_v);
210 err = (*(fb->func_phy_write))(fd, debug, 0, reg, reg_v);
211 }
212
213 return err;
214 }
215
216 int
common_invert_motor(struct function_block * fb,int fd,bool debug,int a_module,char a_motor)217 common_invert_motor(struct function_block *fb, int fd, bool debug, int a_module, char a_motor)
218 {
219 uint8_t b;
220 int err;
221 uint8_t reg, reg_index = 0, reg_offset = 0;
222 int motor_index;
223
224 err = (*(fb->func_clear))(fd, debug);
225 if (! err) {
226 if (a_module == 0) {
227 if (a_motor == 'A') {
228 reg = SCMD_REG_MOTOR_A_INVERT;
229 } else {
230 reg = SCMD_REG_MOTOR_B_INVERT;
231 }
232 err = (*(fb->func_phy_read))(fd, debug, 0, reg, reg, &b);
233 if (!err) {
234 b = b ^ 0x01;
235 err = (*(fb->func_phy_write))(fd, debug, 0, reg, b);
236 }
237 } else {
238 motor_index = (a_module * 2) - 2;
239 if (a_motor == 'B')
240 motor_index++;
241 reg_offset = motor_index / 8;
242 motor_index = motor_index % 8;
243 reg_index = 1 << motor_index;
244 reg = SCMD_REG_INV_2_9 + reg_offset;
245 if (debug)
246 fprintf(stderr,"common_invert_motor: remote invert: motor_index: %d ; reg_offset: %d ; reg_index: %02X ; reg: %02X\n",motor_index,reg_offset,reg_index,reg);
247 err = (*(fb->func_phy_read))(fd, debug, 0, reg, reg, &b);
248 if (!err) {
249 b = b ^ reg_index;
250 err = (*(fb->func_phy_write))(fd, debug, 0, reg, b);
251 }
252 }
253 }
254
255 return err;
256 }
257
258 int
common_bridge_motor(struct function_block * fb,int fd,bool debug,int a_module)259 common_bridge_motor(struct function_block *fb, int fd, bool debug, int a_module)
260 {
261 uint8_t b;
262 int err = 0;
263 uint8_t reg, reg_index = 0, reg_offset = 0;
264 int module_index;
265
266 err = (*(fb->func_clear))(fd, debug);
267 if (! err) {
268 if (a_module == 0) {
269 err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_BRIDGE, SCMD_REG_BRIDGE, &b);
270 if (!err) {
271 b = b ^ 0x01;
272 err = (*(fb->func_phy_write))(fd, debug, 0, SCMD_REG_BRIDGE, b);
273 }
274 } else {
275 module_index = a_module - 1;
276 reg_offset = module_index / 8;
277 module_index = module_index % 8;
278 reg_index = 1 << module_index;
279 reg = SCMD_REG_BRIDGE_SLV_L + reg_offset;
280 if (debug)
281 fprintf(stderr,"common_bridge_motor: remote bridge: module_index: %d ; reg_offset: %d ; reg_index: %02X ; reg: %02X\n",module_index,reg_offset,reg_index,reg);
282 err = (*(fb->func_phy_read))(fd, debug, 0, reg, reg, &b);
283 if (!err) {
284 b = b ^ reg_index;
285 err = (*(fb->func_phy_write))(fd, debug, 0, reg, b);
286 }
287 }
288 }
289
290 return err;
291 }
292
293 int
common_enable_disable(struct function_block * fb,int fd,bool debug,int subcmd)294 common_enable_disable(struct function_block *fb, int fd, bool debug, int subcmd)
295 {
296 int err;
297 uint8_t reg_v;
298
299 if (!(subcmd == SCMD_ENABLE || subcmd == SCMD_DISABLE))
300 return EINVAL;
301
302 err = (*(fb->func_clear))(fd, debug);
303 if (! err) {
304 switch (subcmd) {
305 case SCMD_ENABLE:
306 reg_v = SCMD_DRIVER_ENABLE;
307 break;
308 case SCMD_DISABLE:
309 reg_v = SCMD_DRIVER_DISABLE;
310 break;
311 default:
312 return EINVAL;
313 }
314 err = (*(fb->func_phy_write))(fd, debug, 0, SCMD_REG_DRIVER_ENABLE, reg_v);
315 }
316
317 return err;
318 }
319
320 /* These control commands can take a very long time and the restart
321 * make cause the device to become unresponsive for a bit.
322 */
323 int
common_control_1(struct function_block * fb,int fd,bool debug,int subcmd)324 common_control_1(struct function_block *fb, int fd, bool debug, int subcmd)
325 {
326 int err;
327 uint8_t reg_v;
328
329 if (!(subcmd == SCMD_RESTART || subcmd == SCMD_ENUMERATE))
330 return EINVAL;
331
332 err = (*(fb->func_clear))(fd, debug);
333 if (! err) {
334 switch (subcmd) {
335 case SCMD_RESTART:
336 reg_v = SCMD_CONTROL_1_RESTART;
337 break;
338 case SCMD_ENUMERATE:
339 reg_v = SCMD_CONTROL_1_REENUMERATE;
340 break;
341 default:
342 return EINVAL;
343 }
344 err = (*(fb->func_phy_write))(fd, debug, 0, SCMD_REG_CONTROL_1, reg_v);
345 }
346
347 return err;
348 }
349
350 int
common_get_update_rate(struct function_block * fb,int fd,bool debug,uint8_t * rate)351 common_get_update_rate(struct function_block *fb, int fd, bool debug, uint8_t *rate)
352 {
353 uint8_t b;
354 int err;
355
356 err = (*(fb->func_clear))(fd, debug);
357 if (! err) {
358 err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_UPDATE_RATE, SCMD_REG_UPDATE_RATE, &b);
359 if (!err)
360 *rate = b;
361 }
362
363 return err;
364 }
365
366 int
common_set_update_rate(struct function_block * fb,int fd,bool debug,uint8_t rate)367 common_set_update_rate(struct function_block *fb, int fd, bool debug, uint8_t rate)
368 {
369 int err;
370
371 err = (*(fb->func_clear))(fd, debug);
372 if (! err) {
373 err = (*(fb->func_phy_write))(fd, debug, 0, SCMD_REG_UPDATE_RATE, rate);
374 }
375
376 return err;
377 }
378
379 int
common_force_update(struct function_block * fb,int fd,bool debug)380 common_force_update(struct function_block *fb, int fd, bool debug)
381 {
382 int err;
383
384 err = (*(fb->func_clear))(fd, debug);
385 if (! err) {
386 err = (*(fb->func_phy_write))(fd, debug, 0, SCMD_REG_FORCE_UPDATE, 0x01);
387 }
388
389 return err;
390 }
391
392 int
common_get_ebus_speed(struct function_block * fb,int fd,bool debug,uint8_t * speed)393 common_get_ebus_speed(struct function_block *fb, int fd, bool debug, uint8_t *speed)
394 {
395 uint8_t b;
396 int err;
397
398 err = (*(fb->func_clear))(fd, debug);
399 if (! err) {
400 err = (*(fb->func_phy_read))(fd, debug, 0, SCMD_REG_E_BUS_SPEED, SCMD_REG_E_BUS_SPEED, &b);
401 if (!err)
402 *speed = b;
403 }
404
405 return err;
406 }
407
408 int
common_set_ebus_speed(struct function_block * fb,int fd,bool debug,uint8_t speed)409 common_set_ebus_speed(struct function_block *fb, int fd, bool debug, uint8_t speed)
410 {
411 int err;
412
413 if (speed > 0x03)
414 return EINVAL;
415
416 err = (*(fb->func_clear))(fd, debug);
417 if (! err) {
418 err = (*(fb->func_phy_write))(fd, debug, 0, SCMD_REG_E_BUS_SPEED, speed);
419 }
420
421 return err;
422 }
423
424 int
common_get_lock_state(struct function_block * fb,int fd,bool debug,int ltype,uint8_t * lstate)425 common_get_lock_state(struct function_block *fb, int fd, bool debug, int ltype, uint8_t *lstate)
426 {
427 uint8_t b;
428 uint8_t reg;
429 int err;
430
431 switch (ltype) {
432 case SCMD_LOCAL_USER_LOCK:
433 reg = SCMD_REG_LOCAL_USER_LOCK;
434 break;
435 case SCMD_LOCAL_MASTER_LOCK:
436 reg = SCMD_REG_LOCAL_MASTER_LOCK;
437 break;
438 case SCMD_GLOBAL_USER_LOCK:
439 reg = SCMD_REG_USER_LOCK;
440 break;
441 case SCMD_GLOBAL_MASTER_LOCK:
442 reg = SCMD_REG_MASTER_LOCK;
443 break;
444 default:
445 return EINVAL;
446 }
447
448 err = (*(fb->func_clear))(fd, debug);
449 if (! err) {
450 err = (*(fb->func_phy_read))(fd, debug, 0, reg, reg, &b);
451 if (!err)
452 *lstate = b;
453 }
454
455 return err;
456 }
457
458 int
common_set_lock_state(struct function_block * fb,int fd,bool debug,int ltype,uint8_t lstate)459 common_set_lock_state(struct function_block *fb, int fd, bool debug, int ltype, uint8_t lstate)
460 {
461 uint8_t reg;
462 uint8_t state;
463 int err;
464
465 switch (ltype) {
466 case SCMD_LOCAL_USER_LOCK:
467 reg = SCMD_REG_LOCAL_USER_LOCK;
468 break;
469 case SCMD_LOCAL_MASTER_LOCK:
470 reg = SCMD_REG_LOCAL_MASTER_LOCK;
471 break;
472 case SCMD_GLOBAL_USER_LOCK:
473 reg = SCMD_REG_USER_LOCK;
474 break;
475 case SCMD_GLOBAL_MASTER_LOCK:
476 reg = SCMD_REG_MASTER_LOCK;
477 break;
478 default:
479 return EINVAL;
480 }
481
482 switch (lstate) {
483 case SCMD_LOCK_LOCKED:
484 state = SCMD_ANY_LOCK_LOCKED;
485 break;
486 case SCMD_LOCK_UNLOCK:
487 state = SCMD_MASTER_LOCK_UNLOCKED;
488 if (ltype == SCMD_LOCAL_USER_LOCK ||
489 ltype == SCMD_GLOBAL_USER_LOCK)
490 state = SCMD_USER_LOCK_UNLOCKED;
491 break;
492 default:
493 return EINVAL;
494 }
495
496 err = (*(fb->func_clear))(fd, debug);
497 if (! err) {
498 err = (*(fb->func_phy_write))(fd, debug, 0, reg, state);
499 }
500
501 return err;
502 }
503