void hm2_resolver_process_tram_read(hostmot2_t *hm2, long period) {
int i;
hm2_resolver_instance_t *res;
if (hm2->resolver.num_instances <= 0) return;
// process each resolver instance independently
for (i = 0; i < hm2->resolver.num_resolvers; i ++) {
res = &hm2->resolver.instance[i];
// sanity check
if (*res->hal.pin.scale == 0.0) {
HM2_ERR("resolver.%02d.scale == 0.0, bogus, setting to 1.0\n", i);
*res->hal.pin.scale = 1.0;
}
if (*res->hal.pin.vel_scale == 0.0) {
HM2_ERR("resolver.%02d.velocity-scale == 0.0, bogus, setting to 1.0\n", i);
*res->hal.pin.vel_scale = 1.0;
}
// PROCESS THE REGISTERS, SET THE PINS
res->accum += (__s32)(hm2->resolver.position_reg[i] - res->old_reg );
if ((res->old_reg > hm2->resolver.position_reg[i]) && (res->old_reg - hm2->resolver.position_reg[i] > 0x80000000)){
res->index_cnts++;
if (*res->hal.pin.index_enable){
int r = (res->index_cnts % *res->hal.pin.index_div);
if ((*res->hal.pin.index_div > 1 && r == 1)
|| (*res->hal.pin.index_div == 1 && r == 0)){
res->offset = (res->accum - hm2->resolver.position_reg[i]);
*res->hal.pin.index_enable = 0;
}
}
}
else if ((res->old_reg < hm2->resolver.position_reg[i]) && (hm2->resolver.position_reg[i] - res->old_reg > 0x80000000)){
res->index_cnts--;
if (*res->hal.pin.index_enable && (res->index_cnts % *res->hal.pin.index_div == 0)){
res->offset = (res->accum - hm2->resolver.position_reg[i] + 0x100000000LL);
*res->hal.pin.index_enable = 0;
}
}
if (*res->hal.pin.reset){
res->offset = res->accum;
}
res->old_reg = hm2->resolver.position_reg[i];
*res->hal.pin.angle = hm2->resolver.position_reg[i] / 4294967296.0;
*res->hal.pin.rawcounts = (res->accum >> 8);
*res->hal.pin.count = (res->accum - res->offset) >> 8;
*res->hal.pin.position = (res->accum - res->offset) / 4294967296.0
* *res->hal.pin.scale;
*res->hal.pin.velocity = ((hm2->resolver.velocity_reg[i] / 4294967296.0)
* hm2->resolver.kHz * *res->hal.pin.vel_scale);
*res->hal.pin.error = *hm2->resolver.status_reg & (1 << i);
}
}
int hm2_bspi_setup_chan(char *name, int chan, int cs, int bits, float mhz,
int delay, int cpol, int cpha, int clear, int echo)
{
hostmot2_t *hm2;
rtapi_u32 buff;
int i;
float board_mhz;
i = hm2_get_bspi(&hm2, name);
if (i < 0){
HM2_ERR_NO_LL("Can not find BSPI instance %s.\n", name);
return -1;
}
if (chan<0 || chan > 15){
HM2_ERR("BSPI %s: Channel number (%i) is out of range, BSPI only"
"supports channels 0-15\n", name, chan);
return -1;
}
if (cs > 15 || cs < 0){
HM2_ERR("BSPI %s: Chip Select for channel %i (%i) out of range, only "
"values 0 - 15 are accepted\n", name, chan, cs);
return -1;
}
if (bits > 64 || bits < 1){
HM2_ERR("BSPI %s: Number of bits for chan %i (%i) is out of range, "
"BSPI only supports 1-64 bits\n", name, chan, bits);
return -1;
}
if (delay < 0 || delay > 1e6){
HM2_ERR("The requested frame delay on channel %i of %inS seems "
"rather implausible for an SPI device. Exiting.\n", delay, chan);
return -1;
}
board_mhz = hm2->bspi.instance[i].clock_freq / 1e6;
// reduce clock rate of the FPGA isn't fast enough
if (mhz > board_mhz/2){
mhz=board_mhz/2;
}
buff = (echo != 0) << 31
| (clear != 0) << 30
| ((delay <= 0)? 0x10 : (rtapi_u32)((delay*board_mhz/1000.0)-1) & 0x1f) << 24
| (cs & 0xF) << 16
| (((rtapi_u16)(board_mhz / (mhz * 2) - 1) & 0xF)) << 8
| (cpha != 0) << 7
| (cpol != 0) << 6
| (((rtapi_u16)(bits - 1)) & 0x1F);
HM2_DBG("BSPI %s Channel %i setup %x\n", name, chan, buff);
hm2->bspi.instance[i].cd[chan] = buff;
hm2->bspi.instance[i].conf_flag[chan] = true;
hm2_bspi_force_write(hm2);
return 0;
}
// timeout_s = (timer_counts + 1) / clock_hz
// (timeout_s * clock_hz) - 1 = timer_counts
// (timeout_ns * (1 s/1e9 ns) * clock_hz) - 1 = timer_counts
void hm2_watchdog_force_write(hostmot2_t *hm2) {
u64 tmp;
if (hm2->watchdog.num_instances != 1) return;
if (hm2->watchdog.instance[0].enable == 0) {
// watchdog is disabled, MSb=1 is secret handshake with FPGA
hm2->watchdog.timer_reg[0] = 0x80000000;
} else {
tmp = (hm2->watchdog.instance[0].hal.param.timeout_ns * ((double)hm2->watchdog.clock_frequency / (double)(1000 * 1000 * 1000))) - 1;
if (tmp < 0x80000000) {
hm2->watchdog.timer_reg[0] = tmp;
} else {
// truncate watchdog timeout
tmp = 0x7FFFFFFF;
hm2->watchdog.timer_reg[0] = tmp;
hm2->watchdog.instance[0].hal.param.timeout_ns = (tmp + 1) / ((double)hm2->watchdog.clock_frequency / (double)(1000 * 1000 * 1000));
HM2_ERR("requested watchdog timeout is out of range, setting it to max: %u ns\n", hm2->watchdog.instance[0].hal.param.timeout_ns);
}
}
// set the watchdog timeout (we'll check for i/o errors later)
hm2->llio->write(hm2->llio, hm2->watchdog.timer_addr, hm2->watchdog.timer_reg, (hm2->watchdog.num_instances * sizeof(u32)));
hm2->watchdog.instance[0].written_timeout_ns = hm2->watchdog.instance[0].hal.param.timeout_ns;
hm2->watchdog.instance[0].written_enable = hm2->watchdog.instance[0].enable;
// re-warn the user if their requested timeout is too short
hm2->watchdog.instance[0].warned_about_short_timeout = 0;
// clear the has-bit bit
hm2->llio->write(hm2->llio, hm2->watchdog.status_addr, hm2->watchdog.status_reg, sizeof(u32));
}
void hm2_set_pin_direction(hostmot2_t *hm2, int pin_number, int direction) {
if ((pin_number < 0)
|| (pin_number >= hm2->num_pins)
|| (hm2->ioport.num_instances <= 0)) {
HM2_ERR("hm2_set_pin_direction: invalid pin number %d\n", pin_number);
return;
}
if ((direction != HM2_PIN_DIR_IS_INPUT) && (direction != HM2_PIN_DIR_IS_OUTPUT)) {
HM2_ERR("hm2_set_pin_direction: invalid pin direction 0x%08X\n", direction);
return;
}
hm2->pin[pin_number].direction = direction;
}
int sslbp_write_long(rtapi_u32 addr, rtapi_u32 data){
rtapi_u32 buff = WRITE_REM_LONG_CMD + addr;
HM2WRITE(remote->reg_cs_addr, buff);
HM2WRITE(remote->reg_0_addr, data);
if (doit() < 0){
HM2_ERR("Error in sslbp_write_long, trying to abort\n");
return -1;
}
buff = 0;
HM2WRITE(remote->reg_cs_addr, buff);
return 0;
}
int hm2_bspi_set_write_function(char *name, int (*func)(void *subdata), void *subdata){
hostmot2_t *hm2;
int i;
i = hm2_get_bspi(&hm2, name);
if (i < 0){
HM2_ERR_NO_LL("Can not find BSPI instance %s.\n", name);
return -1;
}
if (func == NULL) {
HM2_ERR("Invalid function pointer passed to "
"hm2_bspi_set_write_function.\n");
return -1;
}
if (subdata == NULL) {
HM2_ERR("Invalid data pointer passed to "
"hm2_bspi_set_write_function.\n");
return -1;
}
hm2->bspi.instance[i].write_function = func;
hm2->bspi.instance[i].subdata = subdata;
return 0;
}
rtapi_u16 sslbp_read_word(rtapi_u32 addr){
rtapi_u32 buff = READ_REM_WORD_CMD + addr;
rtapi_u32 res;
HM2WRITE(remote->reg_cs_addr, buff);
if (doit() < 0){
HM2_ERR("Error in sslbp_read_word, trying to abort\n");
return -1;
}
HM2READ(remote->reg_0_addr, res);
buff = 0;
HM2WRITE(remote->reg_cs_addr, buff);
return (rtapi_u16)res;
}
rtapi_u32 sslbp_read_long(rtapi_u32 addr){
rtapi_u32 buff = READ_REM_LONG_CMD + addr;
rtapi_u32 res=0;
HM2WRITE(remote->reg_cs_addr, buff);
if (doit() < 0){
HM2_ERR("Error in sslbp_read_long, trying to abort\n");
return -1;
}
HM2READ(remote->reg_0_addr, buff);
buff = 0;
HM2WRITE(remote->reg_cs_addr, buff);
return res;
}
int hm2_bspi_write_chan(char* name, int chan, rtapi_u32 val)
{
hostmot2_t *hm2;
rtapi_u32 buff;
int i, r;
i = hm2_get_bspi(&hm2, name);
if (i < 0){
HM2_ERR_NO_LL("Can not find BSPI instance %s.\n", name);
return -1;
}
if (hm2->bspi.instance[i].conf_flag[chan] != true){
HM2_ERR("The selected write channel (%i) on bspi instance %s.\n"
"Has not been configured.\n", chan, name);
return -1;
}
r = hm2->llio->write(hm2->llio, hm2->bspi.instance[i].addr[chan], &buff, sizeof(rtapi_u32));
if (r < 0) {
HM2_ERR("BSPI: hm2->llio->write failure %s\n", name);
}
return r;
}
int sslbp_write_double(rtapi_u32 addr, rtapi_u32 data0, rtapi_u32 data1){
rtapi_u32 buff = WRITE_REM_DOUBLE_CMD + addr;
HM2WRITE(remote->reg_cs_addr, buff);
HM2WRITE(remote->reg_0_addr, data0);
HM2WRITE(remote->reg_1_addr, data1);
if (doit() < 0){
HM2_ERR("Error in sslbp_write_double, trying to abort\n");
return -1;
}
buff = 0;
HM2WRITE(remote->reg_cs_addr, buff);
return 0;
}
int sslbp_read_cookie(void){
rtapi_u32 buff = READ_COOKIE_CMD;
rtapi_u32 res;
HM2WRITE(remote->reg_cs_addr, buff);
if (doit() < 0){
HM2_ERR("Error in sslbp_read_cookie, trying to abort\n");
return -1;
}
HM2READ(remote->reg_0_addr, res);
buff = 0;
HM2WRITE(remote->reg_cs_addr, buff);
return res;
}
void hm2_set_pin_source(hostmot2_t *hm2, int pin_number, int source) {
if ((pin_number < 0)
|| (pin_number >= hm2->num_pins)
|| (hm2->ioport.num_instances <= 0)) {
HM2_ERR("hm2_set_pin_source: invalid pin number %d\n", pin_number);
return;
}
{
hm2_pin_t *pin = &(hm2->pin[pin_number]);
if (source == HM2_PIN_SOURCE_IS_PRIMARY) {
hm2->ioport.alt_source_reg[pin->port_num] &= ~(1 << pin->bit_num);
pin->gtag = pin->primary_tag;
} else if (source == HM2_PIN_SOURCE_IS_SECONDARY) {
hm2->ioport.alt_source_reg[pin->port_num] |= (1 << pin->bit_num);
pin->gtag = pin->sec_tag;
} else {
HM2_ERR("hm2_set_pin_source: invalid pin source 0x%08X\n", source);
return;
}
}
}
void hm2_bspi_prepare_tram_write(hostmot2_t *hm2, long period)
{
int i, r;
int (*func)(void *subdata);
for (i = 0 ; i < hm2->bspi.num_instances ; i++ ){
func = hm2->bspi.instance[i].write_function;
if (func != NULL){
r = func(hm2->bspi.instance[i].subdata);
if(r < 0)
HM2_ERR("BSPI read function @%p failed (returned %d)\n",
func, r);
}
}
}
int hm2_tram_add_bspi_frame(char *name, int chan, rtapi_u32 **wbuff, rtapi_u32 **rbuff)
{
hostmot2_t *hm2;
int i, r;
i = hm2_get_bspi(&hm2, name);
if (i < 0){
HM2_ERR_NO_LL("Can not find BSPI instance %s.\n", name);
return -1;
}
if (hm2->bspi.instance[i].conf_flag[chan] != true){
HM2_ERR("The selected write channel (%i) on bspi instance %s.\n"
"Has not been configured.\n", chan, name);
return -1;
}
if (wbuff != NULL) {
r = hm2_register_tram_write_region(hm2,hm2->bspi.instance[i].addr[chan], sizeof(rtapi_u32),wbuff);
if (r < 0) {
HM2_ERR("Failed to add TRAM write entry for %s.\n", name);
return -1;
}
} else {
HM2_ERR("SPI frame must have a write entry for channel (%i) on %s.\n", chan, name);
return -1;
}
if (rbuff != NULL){
// Don't add a read entry for a no-echo channel
if(!(hm2->bspi.instance[i].cd[chan] & 0x80000000)) {
r = hm2_register_tram_read_region(hm2,hm2->bspi.instance[i].addr[0], sizeof(rtapi_u32),rbuff);
if (r < 0) {
HM2_ERR( "Failed to add TRAM read entry for %s\n", name);
return -1;
}
}
}
return 0;
}
int hm2_allocate_bspi_tram(char* name)
{
hostmot2_t *hm2;
int i, r;
i = hm2_get_bspi(&hm2, name);
if (i < 0){
HM2_ERR_NO_LL("Can not find BSPI instance %s.\n", name);
return -1;
}
r = hm2_allocate_tram_regions(hm2);
if (r < 0) {
HM2_ERR("Failed to register TRAM for BSPI %s\n", name);
return -1;
}
return 0;
}
rtapi_u64 sslbp_read_double(rtapi_u32 addr){
rtapi_u64 res;
rtapi_u32 buff = READ_REM_DOUBLE_CMD + addr;
HM2WRITE(remote->reg_cs_addr, buff);
if (doit() < 0){
HM2_ERR("Error in sslbp_read_double, trying to abort\n");
return -1;
}
HM2READ(remote->reg_1_addr, buff);
res = buff;
res <<= 32;
HM2READ(remote->reg_0_addr, buff);
res += buff;
buff = 0;
HM2WRITE(remote->reg_cs_addr, buff);
return res;
}
// use -1 for tx_mode and rx_mode to leave the mode unchanged
int hm2_uart_setup(char *name, int bitrate, rtapi_s32 tx_mode, rtapi_s32 rx_mode){
hostmot2_t *hm2;
hm2_uart_instance_t *inst = 0;
rtapi_u32 buff;
int i,r;
i = hm2_get_uart(&hm2, name);
if (i < 0){
HM2_ERR_NO_LL("Can not find UART instance %s.\n", name);
return -1;
}
inst = &hm2->uart.instance[i];
buff = (rtapi_u32)((bitrate * 1048576.0)/inst->clock_freq); //20 bits in this version
r = 0;
if (buff != inst->bitrate){
inst->bitrate = buff;
r += hm2->llio->write(hm2->llio, inst->rx_bitrate_addr, &buff, sizeof(rtapi_u32));
r += hm2->llio->write(hm2->llio, inst->tx_bitrate_addr, &buff, sizeof(rtapi_u32));
buff = 0;
r += hm2->llio->write(hm2->llio, inst->rx_mode_addr, &buff, sizeof(rtapi_u32)); // clear faults
r += hm2->llio->write(hm2->llio, inst->rx_fifo_count_addr, &buff, sizeof(rtapi_u32)); // clear fifo
r += hm2->llio->write(hm2->llio, inst->tx_fifo_count_addr, &buff, sizeof(rtapi_u32)); // clear fifo
}
if (tx_mode >= 0) {
buff = ((rtapi_u32)tx_mode) & 0x7f;
r += hm2->llio->write(hm2->llio, inst->tx_mode_addr, &buff, sizeof(rtapi_u32));
}
if (rx_mode >= 0) {
buff = ((rtapi_u32)rx_mode) & 0xff;
r += hm2->llio->write(hm2->llio, inst->rx_mode_addr, &buff, sizeof(rtapi_u32));
}
if (r < 0) {
HM2_ERR("UART: hm2->llio->write failure %s\n", name);
return -1;
}
return 0;
}
// timeout_s = (timer_counts + 1) / clock_hz
// (timeout_s * clock_hz) - 1 = timer_counts
// (timeout_ns * (1 s/1e9 ns) * clock_hz) - 1 = timer_counts
void hm2_watchdog_force_write(hostmot2_t *hm2) {
u64 tmp;
if (hm2->watchdog.num_instances != 1) return;
if (hm2->watchdog.instance[0].enable == 0) {
// watchdog is disabled, MSb=1 is secret handshake with FPGA
hm2->watchdog.timer_reg[0] = 0x80000000;
} else {
#ifdef __KERNEL__
/* some gccs don't include 64-bit integer functions until
linking with gcc, but kbuild links directly with ld; instead,
use kernel-supplied functions; in this case, replace
__udivdi3 with do_div */
tmp = ((u64)hm2->watchdog.instance[0].hal.param.timeout_ns *
hm2->watchdog.clock_frequency);
do_div(tmp,(u64)(1000 * 1000 * 1000)) - 1;
#else
tmp = ((u64)hm2->watchdog.instance[0].hal.param.timeout_ns *
hm2->watchdog.clock_frequency / (1000 * 1000 * 1000)) - 1;
#endif
if (tmp < 0x80000000) {
hm2->watchdog.timer_reg[0] = tmp;
} else {
// truncate watchdog timeout
tmp = 0x7FFFFFFF;
hm2->watchdog.timer_reg[0] = tmp;
hm2->watchdog.instance[0].hal.param.timeout_ns = (tmp + 1) / ((double)hm2->watchdog.clock_frequency / (double)(1000 * 1000 * 1000));
HM2_ERR("requested watchdog timeout is out of range, setting it to max: %u ns\n", hm2->watchdog.instance[0].hal.param.timeout_ns);
}
}
// set the watchdog timeout (we'll check for i/o errors later)
hm2->llio->write(hm2->llio, hm2->watchdog.timer_addr, hm2->watchdog.timer_reg, (hm2->watchdog.num_instances * sizeof(u32)));
hm2->watchdog.instance[0].written_timeout_ns = hm2->watchdog.instance[0].hal.param.timeout_ns;
hm2->watchdog.instance[0].written_enable = hm2->watchdog.instance[0].enable;
// clear the has-bit bit
hm2->llio->write(hm2->llio, hm2->watchdog.status_addr, hm2->watchdog.status_reg, sizeof(u32));
}
void hm2_resolver_write(hostmot2_t *hm2, long period){
//This function needs to handle comms handshaking, so is written as a state machine
static int state = 0;
static u32 cmd_val, data_val;
static u32 timer;
u32 buff;
if (hm2->resolver.num_instances <= 0) return;
switch (state){
case 0: // Idle/waiting
if (*hm2->resolver.hal->pin.excitation_khz < 0){
return;
}
if (*hm2->resolver.hal->pin.excitation_khz != hm2->resolver.written_khz){
if (*hm2->resolver.hal->pin.excitation_khz > 8){
*hm2->resolver.hal->pin.excitation_khz = 10;
hm2->resolver.written_khz = 10;
hm2->resolver.kHz = (hm2->resolver.clock_frequency / 5000);
cmd_val = 0x803;
} else if (*hm2->resolver.hal->pin.excitation_khz > 4){
*hm2->resolver.hal->pin.excitation_khz = 5;
hm2->resolver.written_khz = 5;
hm2->resolver.kHz = (hm2->resolver.clock_frequency / 10000);
cmd_val = 0x802;
} else {
*hm2->resolver.hal->pin.excitation_khz = 2.5;
hm2->resolver.written_khz = 2.5;
hm2->resolver.kHz= (hm2->resolver.clock_frequency / 20000);
cmd_val = 0x801;
}
state = 10;
timer = 0;
return;
}
break;
case 10: // wait for comand register clear before setting params
hm2->llio->read(hm2->llio,hm2->resolver.command_addr,
&buff, sizeof(u32));
if (buff){
timer += period;
if (timer > 1e9){
HM2_ERR("Command not cleared in hm2_resolver, setting aborted");
state = 0;
}
return;
}
hm2->llio->write(hm2->llio, hm2->resolver.data_addr,
&data_val,sizeof(u32));
hm2->llio->write(hm2->llio, hm2->resolver.command_addr,
&cmd_val,sizeof(u32));
state = 20;
timer = 0;
return;
case 20: // wait for command to clear before processing any more params
hm2->llio->read(hm2->llio,hm2->resolver.command_addr,
&buff, sizeof(u32));
if (buff){
timer += period;
if (timer > 1e9){
HM2_ERR("Command not cleared after setting in hm2_resolver");
state = 0;
}
return;
}
state = 0;
return;
break;
default: // That's odd
HM2_ERR("hm2_resolver, unexpected / illegal state in comms state"
"machine");
}
}
int hm2_read_pin_descriptors(hostmot2_t *hm2) {
int i;
int addr;
const u8 DB25[] = {1,14,2,15,3,16,4,17,5,6,7,8,9,10,11,12,13};
hm2->num_pins = hm2->idrom.io_width;
hm2->pin = kmalloc(sizeof(hm2_pin_t) * hm2->num_pins, GFP_KERNEL);
if (hm2->pin == NULL) {
HM2_ERR("out of memory!\n");
return -ENOMEM;
}
addr = hm2->idrom_offset + hm2->idrom.offset_to_pin_desc;
for (i = 0; i < hm2->num_pins; i ++) {
hm2_pin_t *pin = &(hm2->pin[i]);
u32 d;
if (!hm2->llio->read(hm2->llio, addr, &d, sizeof(u32))) {
HM2_ERR("error reading Pin Descriptor %d (at 0x%04x)\n", i, addr);
return -EIO;
}
pin->sec_pin = (d >> 0) & 0x000000FF;
pin->sec_tag = (d >> 8) & 0x000000FF;
pin->sec_unit = (d >> 16) & 0x000000FF;
pin->primary_tag = (d >> 24) & 0x000000FF;
if (pin->primary_tag == 0) {
// oops, found the Zero sentinel before the promised number of pins
HM2_ERR(
"pin %d primary tag is 0 (end-of-list sentinel), expected %d!\n",
i,
hm2->num_pins
);
return -EINVAL;
}
if (pin->primary_tag != HM2_GTAG_IOPORT) {
HM2_ERR(
"pin %d primary tag is %d (%s), not IOPort!\n",
i,
pin->primary_tag,
hm2_get_general_function_name(pin->primary_tag)
);
return -EINVAL;
}
pin->gtag = pin->primary_tag;
pin->port_num = i / hm2->idrom.port_width;
if ((pin->port_num < 0 )
|| (pin->port_num >= hm2->llio->num_ioport_connectors)){
HM2_ERR("hm2_read_pin_descriptors: Calculated port number (%d) is "
"invalid\n", pin->port_pin );
return -EINVAL;
}
pin->bit_num = i % hm2->idrom.port_width;
if ((pin->bit_num < 0 ) || (pin->bit_num > 31)){
HM2_ERR("hm2_read_pin_descriptors: Calculated bit number (%d) is "
"invalid\n", pin->bit_num );
return -EINVAL;
}
switch (hm2->idrom.port_width) {
case 24: /* standard 50 pin 24 I/O cards, just the odd pins */
pin->port_pin = ((i % 24) * 2) + 1;
break;
case 17: /* 25 pin 17 I/O parallel port type cards funny DB25 order */
pin->port_pin = DB25[i % 17];
break;
case 32: /* 5I21 punt on this for now */
pin->port_pin = i + 1;
break;
default:
HM2_ERR("hm2_print_pin_usage: invalid port width %d\n", hm2->idrom.port_width);
}
addr += 4;
}
if (debug_pin_descriptors) {
hm2_print_pin_descriptors(hm2);
}
return 0;
}
int hm2_led_parse_md(hostmot2_t *hm2, int md_index) {
hm2_module_descriptor_t *md = &hm2->md[md_index];
int r;
//
// some standard sanity checks
//
if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 1, 4, 0x0000)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
// LEDs were enumerated during llio setup
if (hm2->llio->num_leds == 0 || hm2->config.num_leds == 0) return 0;
if (hm2->config.num_leds > hm2->llio->num_leds) {
hm2->config.num_leds = hm2->llio->num_leds;
HM2_ERR( "There are only %d LEDs on this board type, defaulting to %d\n",
hm2->llio->num_leds, hm2->config.num_leds );
}
else if (hm2->config.num_leds == -1) {
hm2->config.num_leds = hm2->llio->num_leds;
}
//
// looks good, start initializing
//
// allocate the module-global HAL shared memory
hm2->led.instance = (hm2_led_instance_t *)hal_malloc(hm2->config.num_leds * sizeof(hm2_led_instance_t));
if (hm2->led.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->led.led_reg = (u32 *)kmalloc( sizeof(u32), GFP_KERNEL);
if (hm2->led.led_reg == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->led.led_addr = md->base_address;
// export to HAL
{
int i;
char name[HAL_NAME_LEN+1];
for (i = 0 ; i < hm2->config.num_leds ; i++) {
rtapi_snprintf(name, sizeof(name), "%s.led.CR%02d", hm2->llio->name, i + 1 );
r = hal_pin_bit_new(name, HAL_IN, &(hm2->led.instance[i].led), hm2->llio->comp_id);
if (r < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
}
return 1;
fail1:
kfree(hm2->led.led_reg);
fail0:
return r;
}
}
int sslbp_flash(char *fname){
const struct rtapi_firmware *fw;
struct rtapi_device dev;
int r;
int write_sz, erase_sz;
if (strstr("8i20", remote->name)){
if (hm2->sserial.version < 37){
rtapi_print("SSLBP Version must be at least v37 to flash the 8i20"
"This firmware has v%i. Sorry about that\n"
,hm2->sserial.version);
return -1;
}
}
else if (hm2->sserial.version < 34){
rtapi_print("SSLBP Version must be at least v34. This firmware has v%i"
"\n",hm2->sserial.version);
return -1;
}
if (hm2->sserial.baudrate != 115200){
rtapi_print("To flash firmware the baud rate of the board must be set "
"to 115200 by jumper, and in Hostmot2 using the "
"sserial_baudrate modparam\n");
return -1;
}
//Copied direct from hostmot2.c. A bit of a faff, but seems to be necessary.
memset(&dev, '\0', sizeof(dev));
rtapi_dev_set_name(&dev, hm2->llio->name);
dev.release = setsserial_release;
r = rtapi_device_register(&dev);
if (r != 0) {
HM2_ERR("error with device_register\n");
return -1;
}
r = rtapi_request_firmware(&fw, fname, &dev);
rtapi_device_unregister(&dev);
if (r == -ENOENT) {
HM2_ERR("firmware %s not found\n",fname);
return -1;
}
if (r != 0) {
HM2_ERR("request for firmware %s failed, aborting\n", fname);
return -1;
}
rtapi_print("Firmware size 0x%zx\n", fw->size);
if (setup_start() < 0) goto fail0;
flash_start();
write_sz = 1 << sslbp_read_byte(LBPFLASHWRITESIZELOC);
erase_sz = 1 << sslbp_read_byte(LBPFLASHERASESIZELOC);
HM2_PRINT("Write Size = %x, Erase Size = %x\n", write_sz, erase_sz);
flash_stop();
//Programming Loop
{
int ReservedBlock = 0;
int StartBlock = ReservedBlock + 1;
int blocknum = StartBlock;
int block_start;
int i, j, t;
while (blocknum * erase_sz < fw->size){
block_start = blocknum * erase_sz;
for (t = 0; t < erase_sz && fw->data[block_start + t] == 0 ; t++){ }
if (t < erase_sz){ // found a non-zero byte
flash_start();
sslbp_write_long(LBPFLASHOFFSETLOC, block_start);
sslbp_write_byte(LBPFLASHCOMMITLOC, FLASHERASE_CMD);
if (sslbp_read_cookie() != LBPCOOKIE){
HM2_ERR("Synch failed during block erase: aborting\n");
goto fail0;
}
flash_stop();
HM2_PRINT("Erased block %i\n", blocknum);
flash_start();
for (i = 0; i < erase_sz ; i += write_sz){
sslbp_write_long(LBPFLASHOFFSETLOC, block_start + i);
for (j = 0 ; j < write_sz ; j += 8){
rtapi_u32 data0, data1, m;
m = block_start + i + j;
data0 = (fw->data[m]
+ (fw->data[m + 1] << 8)
+ (fw->data[m + 2] << 16)
+ (fw->data[m + 3] << 24));
data1 = (fw->data[m + 4]
+ (fw->data[m + 5] << 8)
+ (fw->data[m + 6] << 16)
+ (fw->data[m + 7] << 24));
sslbp_write_double(j, data0, data1);
}
sslbp_write_byte(LBPFLASHCOMMITLOC, FLASHWRITE_CMD);
if (sslbp_read_cookie() != LBPCOOKIE){
HM2_ERR("Synch failed during block write: aborting\n");
goto fail0;
}
}
flash_stop();
HM2_PRINT("Wrote block %i\n", blocknum);
}
else // Looks like an all-zeros block
{
HM2_PRINT("Skipped Block %i\n", blocknum);
}
blocknum++;
}
}
rtapi_release_firmware(fw);
return 0;
fail0:
flash_stop();
return -1;
}
int hm2_pwmgen_parse_md(hostmot2_t *hm2, int md_index) {
hm2_module_descriptor_t *md = &hm2->md[md_index];
int r;
//
// some standard sanity checks
//
if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 5, 4, 0x0003)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
if (hm2->pwmgen.num_instances != 0) {
HM2_ERR(
"found duplicate Module Descriptor for %s (inconsistent firmware), not loading driver\n",
hm2_get_general_function_name(md->gtag)
);
return -EINVAL;
}
if (hm2->config.num_pwmgens > md->instances) {
HM2_ERR(
"config.num_pwmgens=%d, but only %d are available, not loading driver\n",
hm2->config.num_pwmgens,
md->instances
);
return -EINVAL;
}
if (hm2->config.num_pwmgens == 0) {
return 0;
}
//
// looks good, start initializing
//
if (hm2->config.num_pwmgens == -1) {
hm2->pwmgen.num_instances = md->instances;
} else {
hm2->pwmgen.num_instances = hm2->config.num_pwmgens;
}
// allocate the module-global HAL shared memory
hm2->pwmgen.hal = (hm2_pwmgen_module_global_t *)hal_malloc(sizeof(hm2_pwmgen_module_global_t));
if (hm2->pwmgen.hal == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->pwmgen.instance = (hm2_pwmgen_instance_t *)hal_malloc(hm2->pwmgen.num_instances * sizeof(hm2_pwmgen_instance_t));
if (hm2->pwmgen.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->pwmgen.clock_frequency = md->clock_freq;
hm2->pwmgen.version = md->version;
hm2->pwmgen.pwm_value_addr = md->base_address + (0 * md->register_stride);
hm2->pwmgen.pwm_mode_addr = md->base_address + (1 * md->register_stride);
hm2->pwmgen.pwmgen_master_rate_dds_addr = md->base_address + (2 * md->register_stride);
hm2->pwmgen.pdmgen_master_rate_dds_addr = md->base_address + (3 * md->register_stride);
hm2->pwmgen.enable_addr = md->base_address + (4 * md->register_stride);
r = hm2_register_tram_write_region(hm2, hm2->pwmgen.pwm_value_addr, (hm2->pwmgen.num_instances * sizeof(rtapi_u32)), &hm2->pwmgen.pwm_value_reg);
if (r < 0) {
HM2_ERR("error registering tram write region for PWM Value register (%d)\n", r);
goto fail0;
}
hm2->pwmgen.pwm_mode_reg = (rtapi_u32 *)rtapi_kmalloc(hm2->pwmgen.num_instances * sizeof(rtapi_u32), RTAPI_GFP_KERNEL);
if (hm2->pwmgen.pwm_mode_reg == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
// export to HAL
// FIXME: r hides the r in enclosing function, and it returns the wrong thing
{
int i;
int r;
char name[HAL_NAME_LEN + 1];
// these hal parameters affect all pwmgen instances
r = hal_param_u32_newf(
HAL_RW,
&(hm2->pwmgen.hal->param.pwm_frequency),
hm2->llio->comp_id,
"%s.pwmgen.pwm_frequency",
hm2->llio->name
);
if (r < 0) {
HM2_ERR("error adding pwmgen.pwm_frequency param, aborting\n");
goto fail1;
}
hm2->pwmgen.hal->param.pwm_frequency = 20000;
hm2->pwmgen.written_pwm_frequency = 0;
r = hal_param_u32_newf(
HAL_RW,
&(hm2->pwmgen.hal->param.pdm_frequency),
hm2->llio->comp_id,
"%s.pwmgen.pdm_frequency",
hm2->llio->name
);
if (r < 0) {
HM2_ERR("error adding pwmgen.pdm_frequency param, aborting\n");
goto fail1;
}
hm2->pwmgen.hal->param.pdm_frequency = 20000;
hm2->pwmgen.written_pdm_frequency = 0;
for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
// pins
rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.value", hm2->llio->name, i);
r = hal_pin_float_new(name, HAL_IN, &(hm2->pwmgen.instance[i].hal.pin.value), hm2->llio->comp_id);
if (r < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.enable", hm2->llio->name, i);
r = hal_pin_bit_new(name, HAL_IN, &(hm2->pwmgen.instance[i].hal.pin.enable), hm2->llio->comp_id);
if (r < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
// parameters
rtapi_snprintf(name, sizeof(name), "%s.pwmgen.%02d.scale", hm2->llio->name, i);
r = hal_param_float_new(name, HAL_RW, &(hm2->pwmgen.instance[i].hal.param.scale), hm2->llio->comp_id);
if (r < 0) {
HM2_ERR("error adding param '%s', aborting\n", name);
goto fail1;
}
r = hal_param_s32_newf(
HAL_RW,
&(hm2->pwmgen.instance[i].hal.param.output_type),
hm2->llio->comp_id,
"%s.pwmgen.%02d.output-type",
hm2->llio->name,
i
);
if (r < 0) {
HM2_ERR("error adding param, aborting\n");
goto fail1;
}
// init hal objects
*(hm2->pwmgen.instance[i].hal.pin.enable) = 0;
*(hm2->pwmgen.instance[i].hal.pin.value) = 0.0;
hm2->pwmgen.instance[i].hal.param.scale = 1.0;
hm2->pwmgen.instance[i].hal.param.output_type = HM2_PWMGEN_OUTPUT_TYPE_PWM;
hm2->pwmgen.instance[i].written_output_type = -666; // force an update at the start
hm2->pwmgen.instance[i].written_enable = -666; // force an update at the start
}
}
return hm2->pwmgen.num_instances;
fail1:
rtapi_kfree(hm2->pwmgen.pwm_mode_reg);
fail0:
hm2->pwmgen.num_instances = 0;
return r;
}
int hm2_watchdog_parse_md(hostmot2_t *hm2, int md_index) {
hm2_module_descriptor_t *md = &hm2->md[md_index];
int r;
//
// some standard sanity checks
//
if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 3, 4, 0)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
if (hm2->watchdog.num_instances != 0) {
HM2_ERR(
"found duplicate Module Descriptor for %s (inconsistent firmware), not loading driver\n",
hm2_get_general_function_name(md->gtag)
);
return -EINVAL;
}
//
// special sanity checks for watchdog
//
if (md->instances != 1) {
HM2_PRINT("MD declares %d watchdogs! only using the first one...\n", md->instances);
}
//
// looks good, start initializing
//
hm2->watchdog.num_instances = 1;
hm2->watchdog.instance = (hm2_watchdog_instance_t *)hal_malloc(hm2->watchdog.num_instances * sizeof(hm2_watchdog_instance_t));
if (hm2->watchdog.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->watchdog.clock_frequency = md->clock_freq;
hm2->watchdog.version = md->version;
hm2->watchdog.timer_addr = md->base_address + (0 * md->register_stride);
hm2->watchdog.status_addr = md->base_address + (1 * md->register_stride);
hm2->watchdog.reset_addr = md->base_address + (2 * md->register_stride);
r = hm2_register_tram_read_region(hm2, hm2->watchdog.status_addr, (hm2->watchdog.num_instances * sizeof(u32)), &hm2->watchdog.status_reg);
if (r < 0) {
HM2_ERR("error registering tram read region for watchdog (%d)\n", r);
goto fail0;
}
r = hm2_register_tram_write_region(hm2, hm2->watchdog.reset_addr, sizeof(u32), &hm2->watchdog.reset_reg);
if (r < 0) {
HM2_ERR("error registering tram write region for watchdog (%d)!\n", r);
goto fail0;
}
//
// allocate memory for register buffers
//
hm2->watchdog.timer_reg = (u32 *)kmalloc(hm2->watchdog.num_instances * sizeof(u32), GFP_KERNEL);
if (hm2->watchdog.timer_reg == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
//
// export to HAL
//
// pins
r = hal_pin_bit_newf(
HAL_IO,
&(hm2->watchdog.instance[0].hal.pin.has_bit),
hm2->llio->comp_id,
"%s.watchdog.has_bit",
hm2->llio->name
);
if (r < 0) {
HM2_ERR("error adding pin, aborting\n");
r = -EINVAL;
goto fail1;
}
// params
r = hal_param_u32_newf(
HAL_RW,
&(hm2->watchdog.instance[0].hal.param.timeout_ns),
hm2->llio->comp_id,
"%s.watchdog.timeout_ns",
hm2->llio->name
);
if (r < 0) {
HM2_ERR("error adding param, aborting\n");
r = -EINVAL;
goto fail1;
}
// the function
{
hal_export_xfunct_args_t xfunct_args = {
.type = FS_XTHREADFUNC,
.funct.x = hm2_pet_watchdog,
.arg = hm2,
.uses_fp = 0,
.reentrant = 0,
.owner_id = hm2->llio->comp_id
};
if ((r = hal_export_xfunctf(&xfunct_args,
"%s.pet_watchdog",
hm2->llio->name)) != 0) {
HM2_ERR("hal_export pet_watchdog failed - %d\n", r);
r = -EINVAL;
goto fail1;
}
}
//
// initialize the watchdog
//
*hm2->watchdog.instance[0].hal.pin.has_bit = 0;
hm2->watchdog.instance[0].hal.param.timeout_ns = 5 * 1000 * 1000; // default timeout is 5 milliseconds
hm2->watchdog.instance[0].enable = 0; // the first pet_watchdog will turn it on
return hm2->watchdog.num_instances;
fail1:
kfree(hm2->watchdog.timer_reg);
fail0:
hm2->watchdog.num_instances = 0;
return r;
}
void hm2_watchdog_print_module(hostmot2_t *hm2) {
int i;
HM2_PRINT("Watchdog: %d\n", hm2->watchdog.num_instances);
if (hm2->watchdog.num_instances <= 0) return;
HM2_PRINT(" clock_frequency: %d Hz (%s MHz)\n", hm2->watchdog.clock_frequency, hm2_hz_to_mhz(hm2->watchdog.clock_frequency));
HM2_PRINT(" version: %d\n", hm2->watchdog.version);
HM2_PRINT(" timer_addr: 0x%04X\n", hm2->watchdog.timer_addr);
HM2_PRINT(" status_addr: 0x%04X\n", hm2->watchdog.status_addr);
HM2_PRINT(" reset_addr: 0x%04X\n", hm2->watchdog.reset_addr);
for (i = 0; i < hm2->watchdog.num_instances; i ++) {
HM2_PRINT(" instance %d:\n", i);
HM2_PRINT(" param timeout_ns = %u\n", hm2->watchdog.instance[i].hal.param.timeout_ns);
HM2_PRINT(" pin has_bit = %d\n", (*hm2->watchdog.instance[i].hal.pin.has_bit));
HM2_PRINT(" reg timer = 0x%08X\n", hm2->watchdog.timer_reg[i]);
}
}
void hm2_pwmgen_handle_pdm_frequency(hostmot2_t *hm2) {
rtapi_u32 dds;
if (hm2->pwmgen.hal->param.pdm_frequency < 1) {
HM2_ERR("pwmgen.pdm_frequency %d is too low, setting to 1\n", hm2->pwmgen.hal->param.pdm_frequency);
hm2->pwmgen.hal->param.pdm_frequency = 1;
}
//
// hal->param.pdm_frequency is the user's desired PDM frequency in Hz
//
// We get to play with PDMClock (frequency at which the PDM counter
// runs) only - PDMBits (number of bits of the PDM Value Register that
// are used to hold the count, this affects resolution) is fixed at 12.
//
// PDMClock is controlled by the 16-bit PDM Master Rate DDS Register:
// PDMClock = ClockHigh * DDS / 65536
//
// PDMBits is 12.
//
// The key equation is:
// PDMFreq = PDMClock / (2^PDMBits)
//
// This can be rewritten as:
// PDMFreq = (ClockHigh * DDS / 65536) / (2^PDMBits)
// PDMFreq = (ClockHigh * DDS) / (65536 * 2^PDMBits)
// PDMFreq = (ClockHigh * DDS) / (65536 * 4096)
//
// The PDMFreq is the frequency at which the 4096-pulse pattern
// repeats. The pulse frequency is 4096 times higher:
//
// PulseFreq = PDMFreq * 4096
// PulseFreq = (ClockHigh * DDS) / 65536
//
// Solve for DDS:
// PDMFreq * (65536 * 4096) = ClockHigh * DDS
// DDS = (PDMFreq * 65536 * 4096) / ClockHigh
//
// PulseFreq = (ClockHigh * DDS) / 65536
// DDS = (PulseFreq * 65536) / ClockHigh
//
// can we do it with 12 bits?
dds = ((double)hm2->pwmgen.hal->param.pdm_frequency * 65536.0) / (double)hm2->pwmgen.clock_frequency;
if (dds == 0) {
// too slow, set frequency to minimum
// From above:
// PulseFreq = (ClockHigh * DDS) / 65536
dds = 1;
hm2->pwmgen.hal->param.pdm_frequency = ((double)hm2->pwmgen.clock_frequency * (double)dds) / 65536.0;
HM2_ERR("min PDM frequency is %d Hz\n", hm2->pwmgen.hal->param.pdm_frequency);
hm2->pwmgen.pdmgen_master_rate_dds_reg = 1;
return;
}
if (dds < 65536) {
// ok
hm2->pwmgen.pdmgen_master_rate_dds_reg = dds;
return;
}
// user wants too much, lower frequency until it'll work with 12 bits
// From above:
// PulseFreq = (ClockHigh * DDS) / 65536
hm2->pwmgen.hal->param.pdm_frequency = ((double)hm2->pwmgen.clock_frequency * 65535.0) / 65536.0;
HM2_ERR("max PDM frequency is %d Hz\n", hm2->pwmgen.hal->param.pdm_frequency);
hm2->pwmgen.pdmgen_master_rate_dds_reg = 65535;
}
int hm2_bspi_parse_md(hostmot2_t *hm2, int md_index)
{
// All this function actually does is allocate memory
// and give the bspi modules names.
//
// some standard sanity checks
//
int i, j, r = -EINVAL;
hm2_module_descriptor_t *md = &hm2->md[md_index];
if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 3, 0x40, 0x0007)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
if (hm2->bspi.num_instances != 0) {
HM2_ERR(
"found duplicate Module Descriptor for %s (inconsistent "
"firmware), not loading driver\n",
hm2_get_general_function_name(md->gtag)
);
return -EINVAL;
}
if (hm2->config.num_bspis > md->instances) {
HM2_ERR(
"config defines %d bspis, but only %d are available, "
"not loading driver\n",
hm2->config.num_bspis,
md->instances
);
return -EINVAL;
}
if (hm2->config.num_bspis == 0) {
return 0;
}
//
// looks good, start initializing
//
if (hm2->config.num_bspis == -1) {
hm2->bspi.num_instances = md->instances;
} else {
hm2->bspi.num_instances = hm2->config.num_bspis;
}
hm2->bspi.instance = (hm2_bspi_instance_t *)hal_malloc(hm2->bspi.num_instances
* sizeof(hm2_bspi_instance_t));
if (hm2->bspi.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
for (i = 0 ; i < hm2->bspi.num_instances ; i++){
hm2_bspi_instance_t *chan = &hm2->bspi.instance[i];
chan->clock_freq = md->clock_freq;
r = sprintf(chan->name, "%s.bspi.%01d", hm2->llio->name, i);
HM2_PRINT("created Buffered SPI function %s.\n", chan->name);
chan->base_address = md->base_address + i * md->instance_stride;
chan->register_stride = md->register_stride;
chan->instance_stride = md->instance_stride;
chan->cd_addr = md->base_address + md->register_stride + i * sizeof(rtapi_u32);
chan->count_addr = md->base_address + 2 * md->register_stride + i * sizeof(rtapi_u32);
for (j = 0 ; j < 16 ; j++ ){
chan->addr[j] = chan->base_address + j * sizeof(rtapi_u32);
}
}
return hm2->bspi.num_instances;
fail0:
return r;
}
void hm2_pwmgen_handle_pwm_frequency(hostmot2_t *hm2) {
rtapi_u32 dds;
if (hm2->pwmgen.hal->param.pwm_frequency < 1) {
HM2_ERR("pwmgen.pwm_frequency %d is too low, setting to 1\n", hm2->pwmgen.hal->param.pwm_frequency);
hm2->pwmgen.hal->param.pwm_frequency = 1;
}
//
// hal->param.pwm_frequency is the user's desired PWM frequency in Hz
//
// We get to play with PWMClock (frequency at which the PWM counter
// runs) and PWMBits (number of bits of the PWM Value Register that
// are used to hold the count, this affects resolution).
//
// PWMClock is controlled by the 16-bit PWM Master Rate DDS Register:
// PWMClock = ClockHigh * DDS / 65536
//
// PWMBits is 9-12. More is better (higher resolution).
//
// The key equation is:
// PWMFreq = PWMClock / (2^PWMBits)
//
// This can be rewritten as:
// PWMFreq = (ClockHigh * DDS / 65536) / (2^PWMBits)
// PWMFreq = (ClockHigh * DDS) / (65536 * 2^PWMBits)
//
// Solve for DDS:
// PWMFreq * (65536 * 2^PWMBits) = ClockHigh * DDS
// DDS = (PWMFreq * 65536 * 2^PWMBits) / ClockHigh
//
// can we do it with 12 bits?
dds = ((double)hm2->pwmgen.hal->param.pwm_frequency * 65536.0 * 4096.0) / (double)hm2->pwmgen.clock_frequency;
if (dds < 65536) {
hm2->pwmgen.pwmgen_master_rate_dds_reg = dds;
hm2->pwmgen.pwm_bits = 12;
return;
}
// try 11 bits
dds = ((double)hm2->pwmgen.hal->param.pwm_frequency * 65536.0 * 2048.0) / (double)hm2->pwmgen.clock_frequency;
if (dds < 65536) {
hm2->pwmgen.pwmgen_master_rate_dds_reg = dds;
hm2->pwmgen.pwm_bits = 11;
return;
}
// try 10 bits
dds = ((double)hm2->pwmgen.hal->param.pwm_frequency * 65536.0 * 1024.0) / (double)hm2->pwmgen.clock_frequency;
if (dds < 65536) {
hm2->pwmgen.pwmgen_master_rate_dds_reg = dds;
hm2->pwmgen.pwm_bits = 10;
return;
}
// try 9 bits
dds = ((double)hm2->pwmgen.hal->param.pwm_frequency * 65536.0 * 512.0) / (double)hm2->pwmgen.clock_frequency;
if (dds < 65536) {
hm2->pwmgen.pwmgen_master_rate_dds_reg = dds;
hm2->pwmgen.pwm_bits = 9;
return;
}
// no joy, lower frequency until it'll work with 9 bits
// From above:
// PWMFreq = (ClockHigh * DDS) / (65536 * 2^PWMBits)
hm2->pwmgen.hal->param.pwm_frequency = ((double)hm2->pwmgen.clock_frequency * 65535.0) / (65536.0 * 512.0);
HM2_ERR("max PWM frequency is %d Hz\n", hm2->pwmgen.hal->param.pwm_frequency);
hm2->pwmgen.pwmgen_master_rate_dds_reg = 65535;
hm2->pwmgen.pwm_bits = 9;
}
int hm2_resolver_parse_md(hostmot2_t *hm2, int md_index) {
hm2_module_descriptor_t *md = &hm2->md[md_index];
int i, r = 0;
int resolvers_per_instance;
//
// some standard sanity checks
//
if ( ! hm2_md_is_consistent_or_complain(hm2, md_index, 0, 5, 4, 0x001F)) {
HM2_ERR("inconsistent resolver Module Descriptor!\n");
return -EINVAL;
}
if (hm2->resolver.num_instances != 0) {
HM2_ERR(
"found duplicate Module Descriptor for %s (inconsistent firmware), not loading driver\n",
hm2_get_general_function_name(md->gtag)
);
return -EINVAL;
}
resolvers_per_instance = hm2_resolver_get_param(2); // just returns 6 at the moment
if (hm2->config.num_resolvers > (md->instances * resolvers_per_instance)) {
HM2_ERR(
"config.num_resolvers=%d, but only %d are available, not loading driver\n",
hm2->config.num_resolvers,
md->instances * resolvers_per_instance);
return -EINVAL;
}
if (hm2->config.num_resolvers == 0) {
return 0;
}
//
// looks good, start initializing
//
/*At the moment it is not clear if there will ever be more than one resolver
instance. If there were to be more than one then they would need to have
a different base-address, and this code would need to be re-enterable.
A bridge to cross when we come to it */
if (hm2->config.num_resolvers == -1) {
hm2->resolver.num_resolvers = md->instances * resolvers_per_instance;
hm2->resolver.num_instances = md->instances;
} else {
hm2->resolver.num_resolvers = hm2->config.num_resolvers;
hm2->resolver.num_instances = md->instances;
}
hm2->resolver.hal = (hm2_resolver_global_t *)hal_malloc(
sizeof(hm2_resolver_global_t));
if (hm2->resolver.hal == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
hm2->resolver.instance = (hm2_resolver_instance_t *)hal_malloc(
hm2->resolver.num_resolvers * sizeof(hm2_resolver_instance_t));
if (hm2->resolver.instance == NULL) {
HM2_ERR("out of memory!\n");
r = -ENOMEM;
goto fail0;
}
for (i = 0 ; i < hm2->resolver.num_instances ; i++ ){
hm2->resolver.stride = md->register_stride;
hm2->resolver.clock_frequency = md->clock_freq;
hm2->resolver.version = md->version;
hm2->resolver.command_addr = md->base_address + (0 * md->register_stride);
hm2->resolver.data_addr = md->base_address + (1 * md->register_stride);
hm2->resolver.status_addr = md->base_address + (2 * md->register_stride);
hm2->resolver.velocity_addr = md->base_address + (3 * md->register_stride);
hm2->resolver.position_addr = md->base_address + (4 * md->register_stride);
// If there were multiple resolver function instances, this would need
// to be the number of resolvers for that particular instance
r = hm2_register_tram_read_region(hm2, hm2->resolver.status_addr,
sizeof(u32),
&hm2->resolver.status_reg);
r += hm2_register_tram_read_region(hm2, hm2->resolver.position_addr,
(hm2->resolver.num_resolvers * sizeof(u32)),
&hm2->resolver.position_reg);
r += hm2_register_tram_read_region(hm2, hm2->resolver.velocity_addr,
(hm2->resolver.num_resolvers * sizeof(u32)),
(u32**)&hm2->resolver.velocity_reg);
if (r < 0) {
HM2_ERR("error registering tram read region for Resolver "
"register (%d)\n", i);
goto fail1;
}
}
// export the resolvers to HAL
{
int i;
int ret;
char name[HAL_NAME_LEN + 1];
rtapi_snprintf(name, sizeof(name), "%s.resolver.excitation-khz",
hm2->llio->name);
ret= hal_pin_float_new(name, HAL_IO,
&(hm2->resolver.hal->pin.excitation_khz),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
for (i = 0; i < hm2->resolver.num_resolvers; i ++) {
// pins
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.position",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.position),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.angle",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.angle),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.velocity",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.velocity),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.count",
hm2->llio->name, i);
ret= hal_pin_s32_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.count),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.rawcounts",
hm2->llio->name, i);
ret= hal_pin_s32_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.rawcounts),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.reset",
hm2->llio->name, i);
ret= hal_pin_bit_new(name, HAL_IN,
&(hm2->resolver.instance[i].hal.pin.reset),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.index-enable",
hm2->llio->name, i);
ret= hal_pin_bit_new(name, HAL_IO,
&(hm2->resolver.instance[i].hal.pin.index_enable),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.error",
hm2->llio->name, i);
ret= hal_pin_bit_new(name, HAL_OUT,
&(hm2->resolver.instance[i].hal.pin.error),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
// parameters
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.scale",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_IO,
&(hm2->resolver.instance[i].hal.pin.scale),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.velocity-scale",
hm2->llio->name, i);
ret= hal_pin_float_new(name, HAL_IO,
&(hm2->resolver.instance[i].hal.pin.vel_scale),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
rtapi_snprintf(name, sizeof(name), "%s.resolver.%02d.index-divisor",
hm2->llio->name, i);
ret= hal_pin_u32_new(name, HAL_RW,
&(hm2->resolver.instance[i].hal.pin.index_div),
hm2->llio->comp_id);
if (ret < 0) {
HM2_ERR("error adding pin '%s', aborting\n", name);
goto fail1;
}
//
// init the hal objects that need it
// the things not initialized here will be set by hm2_resolver_tram_init()
//
*hm2->resolver.instance[i].hal.pin.reset = 0;
*hm2->resolver.instance[i].hal.pin.scale = 1.0;
*hm2->resolver.instance[i].hal.pin.vel_scale = 1.0;
*hm2->resolver.instance[i].hal.pin.index_div = 1;
*hm2->resolver.hal->pin.excitation_khz = -1; // don't-write
hm2->resolver.kHz = (hm2->resolver.clock_frequency / 5000);
}
}
return hm2->resolver.num_instances;
fail1:
// This is where we would kfree anything kmalloced.
fail0:
hm2->resolver.num_instances = 0;
return r;
}
int hm2_dpll_parse_md(hostmot2_t *hm2, int md_index) {
hm2_module_descriptor_t *md = &hm2->md[md_index];
int r;
//
// some standard sanity checks
//
if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 7, 4, 0x0000)) {
HM2_ERR("inconsistent Module Descriptor!\n");
return -EINVAL;
}
if (hm2->config.num_dplls == 0) return 0;
if (hm2->config.num_dplls > md->instances) {
hm2->dpll.num_instances = md->instances;
HM2_ERR( "There are only %d dplls on this board type, using %d\n",
md->instances, md->instances );
} else if (hm2->config.num_dplls == -1) {
hm2->dpll.num_instances = md->instances;
} else hm2->dpll.num_instances = hm2->config.num_dplls;
//
// looks good, start initializing
//
hm2->dpll.clock_frequency = md->clock_freq;
hm2->dpll.base_rate_addr = md->base_address + 0 * md->register_stride;
hm2->dpll.phase_err_addr = md->base_address + 1 * md->register_stride;
hm2->dpll.control_reg0_addr = md->base_address + 2 * md->register_stride;
hm2->dpll.control_reg1_addr = md->base_address + 3 * md->register_stride;
hm2->dpll.timer_12_addr = md->base_address + 4 * md->register_stride;
hm2->dpll.timer_34_addr = md->base_address + 5 * md->register_stride;
hm2->dpll.hm2_dpll_sync_addr = md->base_address + 6 * md->register_stride;
// export to HAL
hm2->dpll.pins = hal_malloc(sizeof(hm2_dpll_pins_t));
r = hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->time1_us),
hm2->llio->comp_id, "%s.dpll.01.timer-us", hm2->llio->name);
r += hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->time2_us),
hm2->llio->comp_id, "%s.dpll.02.timer-us", hm2->llio->name);
r += hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->time3_us),
hm2->llio->comp_id, "%s.dpll.03.timer-us", hm2->llio->name);
r += hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->time4_us),
hm2->llio->comp_id, "%s.dpll.04.timer-us", hm2->llio->name);
r += hal_pin_float_newf(HAL_IN, &(hm2->dpll.pins->base_freq),
hm2->llio->comp_id, "%s.dpll.base-freq-khz", hm2->llio->name);
r += hal_pin_float_newf(HAL_OUT, &(hm2->dpll.pins->phase_error),
hm2->llio->comp_id, "%s.dpll.phase-error-us", hm2->llio->name);
r += hal_pin_u32_newf(HAL_IN, &(hm2->dpll.pins->time_const),
hm2->llio->comp_id, "%s.dpll.time-const", hm2->llio->name);
r += hal_pin_u32_newf(HAL_IN, &(hm2->dpll.pins->plimit),
hm2->llio->comp_id, "%s.dpll.plimit", hm2->llio->name);
r += hal_pin_u32_newf(HAL_OUT, &(hm2->dpll.pins->ddssize),
hm2->llio->comp_id, "%s.dpll.ddsize", hm2->llio->name);
r += hal_pin_u32_newf(HAL_OUT, &(hm2->dpll.pins->prescale),
hm2->llio->comp_id, "%s.dpll.prescale", hm2->llio->name);
if (r < 0) {
HM2_ERR("error adding hm2_dpll timer pins, Aborting\n");
goto fail0;
}
*hm2->dpll.pins->time1_us = 100.0;
*hm2->dpll.pins->time2_us = 100.0;
*hm2->dpll.pins->time3_us = 100.0;
*hm2->dpll.pins->time4_us = 100.0;
*hm2->dpll.pins->prescale = 1;
*hm2->dpll.pins->base_freq = -1; // An indication it needs init
*hm2->dpll.pins->time_const = 0xA000;
*hm2->dpll.pins->plimit = 0x400000;
r = hm2_register_tram_read_region(hm2, hm2->dpll.hm2_dpll_sync_addr,
sizeof(u32), &hm2->dpll.hm2_dpll_sync_reg);
if (r < 0) {
HM2_ERR("Error registering tram synch write. Aborting\n");
goto fail0;
}
r = hm2_register_tram_read_region(hm2, hm2->dpll.control_reg1_addr,
sizeof(u32), &hm2->dpll.control_reg1_read);
if (r < 0) {
HM2_ERR("Error registering dpll control reg 1. Aborting\n");
goto fail0;
}
return hm2->dpll.num_instances;
fail0:
return r;
}
void hm2_pwmgen_force_write(hostmot2_t *hm2) {
int i;
rtapi_u32 pwm_width;
if (hm2->pwmgen.num_instances == 0) return;
hm2_pwmgen_handle_pwm_frequency(hm2);
hm2_pwmgen_handle_pdm_frequency(hm2);
switch (hm2->pwmgen.pwm_bits) {
case 9: {
pwm_width = 0x00;
break;
}
case 10: {
pwm_width = 0x01;
break;
}
case 11: {
pwm_width = 0x02;
break;
}
case 12: {
pwm_width = 0x03;
break;
}
default: {
// this should never happen
HM2_ERR("invalid pwmgen.bits=%d, resetting to 9\n", hm2->pwmgen.pwm_bits);
hm2->pwmgen.pwm_bits = 9;
pwm_width = 0x00;
break;
}
}
for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
rtapi_u32 double_buffered;
hm2->pwmgen.pwm_mode_reg[i] = pwm_width;
switch (hm2->pwmgen.instance[i].hal.param.output_type) {
case HM2_PWMGEN_OUTPUT_TYPE_PWM: {
// leave the Output Mode bits 0
double_buffered = 1;
break;
}
case HM2_PWMGEN_OUTPUT_TYPE_UP_DOWN: {
hm2->pwmgen.pwm_mode_reg[i] |= 0x2 << 3;
double_buffered = 1;
break;
}
case HM2_PWMGEN_OUTPUT_TYPE_PDM: {
hm2->pwmgen.pwm_mode_reg[i] |= 0x3 << 3;
double_buffered = 0;
break;
}
case HM2_PWMGEN_OUTPUT_TYPE_PWM_SWAPPED: { // Dir & PWM (ie with the output pins swapped), "for locked antiphase"
hm2->pwmgen.pwm_mode_reg[i] |= 0x1 << 3;
double_buffered = 1;
break;
}
default: { // unknown pwm mode! complain and switch to pwm/dir
HM2_ERR(
"invalid pwmgen output_type %d requested\n",
hm2->pwmgen.instance[i].hal.param.output_type
);
HM2_ERR(
"supported .output-type values are: %d (PWM & Dir), %d (Up & Down), %d (PDM & Dir), and %d (Dir & PWM)\n",
HM2_PWMGEN_OUTPUT_TYPE_PWM,
HM2_PWMGEN_OUTPUT_TYPE_UP_DOWN,
HM2_PWMGEN_OUTPUT_TYPE_PDM,
HM2_PWMGEN_OUTPUT_TYPE_PWM_SWAPPED
);
HM2_ERR("switching to 1 (PWM & Dir)\n");
hm2->pwmgen.instance[i].hal.param.output_type = HM2_PWMGEN_OUTPUT_TYPE_PWM;
double_buffered = 1;
// leave the Output Mode bits 0
break;
}
}
hm2->pwmgen.pwm_mode_reg[i] |= (double_buffered << 5);
}
// update enable register
hm2->pwmgen.enable_reg = 0;
for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
if (*(hm2->pwmgen.instance[i].hal.pin.enable)) {
hm2->pwmgen.enable_reg |= (1 << i);
}
}
hm2->llio->write(hm2->llio, hm2->pwmgen.pwm_mode_addr, hm2->pwmgen.pwm_mode_reg, (hm2->pwmgen.num_instances * sizeof(rtapi_u32)));
hm2->llio->write(hm2->llio, hm2->pwmgen.enable_addr, &hm2->pwmgen.enable_reg, sizeof(rtapi_u32));
hm2->llio->write(hm2->llio, hm2->pwmgen.pwmgen_master_rate_dds_addr, &hm2->pwmgen.pwmgen_master_rate_dds_reg, sizeof(rtapi_u32));
hm2->llio->write(hm2->llio, hm2->pwmgen.pdmgen_master_rate_dds_addr, &hm2->pwmgen.pdmgen_master_rate_dds_reg, sizeof(rtapi_u32));
if ((*hm2->llio->io_error) != 0) return;
for (i = 0; i < hm2->pwmgen.num_instances; i ++) {
hm2->pwmgen.instance[i].written_output_type = hm2->pwmgen.instance[i].hal.param.output_type;
hm2->pwmgen.instance[i].written_enable = *hm2->pwmgen.instance[i].hal.pin.enable;
}
hm2->pwmgen.written_pwm_frequency = hm2->pwmgen.hal->param.pwm_frequency;
hm2->pwmgen.written_pdm_frequency = hm2->pwmgen.hal->param.pdm_frequency;
}
