Home | History | Annotate | Line # | Download | only in scmdctl 
      1 /*	$NetBSD: common.c,v 1.1 2021/12/07 17:39:55 brad Exp $	*/
      2 
      3 /*
      4  * Copyright (c) 2021 Brad Spencer <brad (at) 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