xref: /netbsd-src/usr.bin/scmdctl/common.c (revision 53b02e147d4ed531c0d2a5ca9b3e8026ba3e99b5) 
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
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 
59 int common_clear(struct function_block *fb, int fd, bool debug)
60 {
61 	return (*(fb->func_clear))(fd, debug);
62 }
63 
64 int
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 
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
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
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
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
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
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
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
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
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
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
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
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
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
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