diff --git a/src/hal/drivers/mesa-hostmot2/abs_encoder.c b/src/hal/drivers/mesa-hostmot2/abs_encoder.c index 4ee291240b1..6e54c63039f 100644 --- a/src/hal/drivers/mesa-hostmot2/abs_encoder.c +++ b/src/hal/drivers/mesa-hostmot2/abs_encoder.c @@ -268,19 +268,26 @@ int hm2_absenc_parse_format(hm2_sserial_remote_t *chan, hm2_absenc_format_t *de while(*format){ if (*format == '%'){ - int q = simple_strtol(++format, &format, 0); + long q = simple_strtol(++format, &format, 0); if (q == 0){ HM2_ERR_NO_LL("Invalid field length specification, you may " "not get the pins you expected\n"); } else if (strchr("bBuUsSeEfFpPgGhHlLmM", *format)){ + hm2_sserial_data_t *confs; hm2_sserial_data_t *conf; - chan->num_confs++; - chan->confs = (hm2_sserial_data_t *)rtapi_krealloc(chan->confs, - chan->num_confs * sizeof(hm2_sserial_data_t), + + if (q < 0 || q > RTAPI_UINT8_MAX) { + HM2_ERR_NO_LL("Invalid field length %ld\n", q); + return -EINVAL; + } + confs = rtapi_krealloc(chan->confs, + (chan->num_confs + 1) * sizeof(*confs), RTAPI_GFP_KERNEL); + if (confs == NULL) return -ENOMEM; + chan->confs = confs; - conf = &(chan->confs[chan->num_confs - 1]); + conf = &(chan->confs[chan->num_confs++]); conf->DataDir = LBP_IN; conf->DataLength = q; rtapi_strxcpy(conf->NameString, name); @@ -369,6 +376,10 @@ int hm2_absenc_parse_format(hm2_sserial_remote_t *chan, hm2_absenc_format_t *de else { // Not a % format, append name + if (nameptr == &name[sizeof(name) - 1]) { + HM2_ERR_NO_LL("Absolute encoder field name is too long\n"); + return -EINVAL; + } *nameptr++ = *format++; *nameptr = 0; } @@ -412,12 +423,6 @@ int hm2_absenc_parse_md(hostmot2_t *hm2, int md_index) { if (hm2->absenc.num_chans == 0) { // first time though hm2->absenc.clock_frequency = md->clock_freq; - hm2->absenc.ssi_busy_flags = rtapi_kmalloc(sizeof(rtapi_u32), RTAPI_GFP_KERNEL); - *hm2->absenc.ssi_busy_flags = 0; - hm2->absenc.biss_busy_flags = rtapi_kmalloc(sizeof(rtapi_u32), RTAPI_GFP_KERNEL); - *hm2->absenc.biss_busy_flags = 0; - hm2->absenc.fabs_busy_flags = rtapi_kmalloc(sizeof(rtapi_u32), RTAPI_GFP_KERNEL); - *hm2->absenc.fabs_busy_flags = 0; } for (index = 0 ; index < md->instances ; index ++){ @@ -434,12 +439,18 @@ int hm2_absenc_parse_md(hostmot2_t *hm2, int md_index) { goto fail1; } if (index == def->index && md->gtag == def->gtag){ + hm2_sserial_remote_t *chans; + has_format = true; - hm2->absenc.num_chans += 1; - hm2->absenc.chans = rtapi_krealloc(hm2->absenc.chans, - hm2->absenc.num_chans * sizeof(hm2_sserial_remote_t), + chans = rtapi_krealloc(hm2->absenc.chans, + (hm2->absenc.num_chans + 1) * sizeof(*chans), RTAPI_GFP_KERNEL); - chan = &hm2->absenc.chans[hm2->absenc.num_chans - 1]; + if (chans == NULL) { + HM2_ERR("out of memory allocating absolute encoder channel\n"); + goto fail1; + } + hm2->absenc.chans = chans; + chan = &hm2->absenc.chans[hm2->absenc.num_chans++]; memset(chan, 0, sizeof(hm2_sserial_remote_t)); chan->index = index; chan->myinst = md->gtag; @@ -529,6 +540,8 @@ void hm2_absenc_process_tram_read(hostmot2_t *hm2, long period) { err_tag[i] = 1; } } + // These pointers are supplied by hm2_absenc_register_tram() whenever + // the corresponding global-start address enables this dereference. if (((chan->myinst == HM2_GTAG_SSI) && (*hm2->absenc.ssi_busy_flags & (1 << chan->index))) || ((chan->myinst == HM2_GTAG_BISS) diff --git a/src/hal/drivers/mesa-hostmot2/setsserial.c b/src/hal/drivers/mesa-hostmot2/setsserial.c index c7ee787775c..e5ac5eb342c 100644 --- a/src/hal/drivers/mesa-hostmot2/setsserial.c +++ b/src/hal/drivers/mesa-hostmot2/setsserial.c @@ -176,7 +176,7 @@ int sslbp_read_cookie(void){ return res; } -rtapi_u8 sslbp_read_byte(rtapi_u32 addr){ +int sslbp_read_byte(rtapi_u32 addr){ rtapi_u32 buff = READ_REM_BYTE_CMD + addr; rtapi_u32 res; HM2WRITE(remote->reg_cs_addr, buff); @@ -303,6 +303,7 @@ int sslbp_flash(char *fname){ struct rtapi_device dev; int r; unsigned write_sz, erase_sz; + int write_exp, erase_exp; if (strstr("8i20", remote->name)){ if (hm2->sserial.version < 37){ @@ -348,8 +349,17 @@ int sslbp_flash(char *fname){ if (setup_start() < 0) goto fail0; flash_start(); - write_sz = 1u << sslbp_read_byte(LBPFLASHWRITESIZELOC); - erase_sz = 1u << sslbp_read_byte(LBPFLASHERASESIZELOC); + write_exp = sslbp_read_byte(LBPFLASHWRITESIZELOC); + erase_exp = sslbp_read_byte(LBPFLASHERASESIZELOC); + if (write_exp < 3 || write_exp >= (int)(sizeof(write_sz) * 8) + || erase_exp < 0 || erase_exp < write_exp + || erase_exp >= (int)(sizeof(erase_sz) * 8)) { + HM2_ERR("Invalid flash geometry exponents write=%d erase=%d\n", + write_exp, erase_exp); + goto fail0; + } + write_sz = 1u << write_exp; + erase_sz = 1u << erase_exp; HM2_PRINT("Write Size = %x, Erase Size = %x\n", write_sz, erase_sz); flash_stop(); @@ -412,6 +422,7 @@ int sslbp_flash(char *fname){ fail0: flash_stop(); + rtapi_release_firmware(fw); return -1; } diff --git a/src/hal/drivers/mesa-hostmot2/sserial.c b/src/hal/drivers/mesa-hostmot2/sserial.c index bf70a0cc307..2eb1d795d07 100644 --- a/src/hal/drivers/mesa-hostmot2/sserial.c +++ b/src/hal/drivers/mesa-hostmot2/sserial.c @@ -28,11 +28,6 @@ #include "hostmot2.h" -// Local definition of labs() because RTAI compile barfs on using labs(). It -// could be optimized using compiler built-ins, but the single use in here is -// not critical. -static inline rtapi_s64 xlabs(rtapi_s64 x) { return x < 0 ? -x : x; } - int getbits(hm2_sserial_remote_t *chan, rtapi_u64 *val, int start, int len){ //load the bits from the registers in to bit 0+ of *val int i; @@ -308,6 +303,9 @@ int hm2_sserial_get_bytes(hostmot2_t *hm2, *(ptr++) = (unsigned char)data; } + if (string < 0) { + *ptr = '\0'; + } return addr; } @@ -431,8 +429,6 @@ int hm2_sserial_create_params(hostmot2_t *hm2, hm2_sserial_remote_t *chan){ for (i = 0 ; i < chan->num_globals ; i++){ global = chan->globals[i]; - r = 0; - hal_dir = (global.DataDir == LBP_IN) ? HAL_RO : HAL_RW; chan->params[i].type = global.DataType; @@ -525,21 +521,23 @@ int hm2_sserial_get_globals_list(hostmot2_t *hm2, hm2_sserial_remote_t *chan){ } // process is a subset of global. The only way to tell is to compare for (i = 0; i < chan->num_confs ; i ++) { - if (chan->confs[i].ParmAddr == data.ParmAddr){i = 1000;} + if (chan->confs[i].ParmAddr == data.ParmAddr) break; } - if (data.RecordType == LBP_DATA && i < 1000) { + if (data.RecordType == LBP_DATA && i == chan->num_confs) { + hm2_sserial_data_t *globals; + addr = hm2_sserial_get_bytes(hm2, chan, &(data.UnitString), addr, -1); if (addr < 0){ return -EINVAL;} addr = hm2_sserial_get_bytes(hm2, chan, &(data.NameString), addr, -1); if (addr < 0){ return -EINVAL;} HM2_DBG("Global: %s RecordType: %02X Datatype: %02X Dir: %02X Addr: %04X Length: %i\n", data.NameString, data.RecordType, data.DataType, data.DataDir, data.ParmAddr, data.DataLength); - chan->num_globals++; - chan->globals = (hm2_sserial_data_t *) - rtapi_krealloc(chan->globals, - chan->num_globals * sizeof(hm2_sserial_data_t), + globals = rtapi_krealloc(chan->globals, + (chan->num_globals + 1) * sizeof(*globals), RTAPI_GFP_KERNEL); - chan->globals[chan->num_globals - 1] = data; + if (globals == NULL) return -ENOMEM; + chan->globals = globals; + chan->globals[chan->num_globals++] = data; } else if (data.RecordType== LBP_MODE){ char * type; @@ -642,6 +640,8 @@ int hm2_sserial_parse_md(hostmot2_t *hm2, int md_index){ r = -ENOMEM; goto fail0; } + memset(hm2->sserial.instance, 0, + hm2->sserial.num_instances * sizeof(*hm2->sserial.instance)); // We can't create the pins until we know what is on each channel, and // can't communicate until the pin directions are set up. @@ -678,6 +678,10 @@ int hm2_sserial_parse_md(hostmot2_t *hm2, int md_index){ // Now iterate through the sserial instances, seeing what is on the enabled pins. for (i = 0 ; i < md->instances ; i++) { + // Keep probing physical ports until the configured number of populated + // ports has been found. Empty lower-numbered ports do not consume an + // entry in the compact instance array. + if (count >= hm2->sserial.num_instances) break; hm2_sserial_instance_t *inst = &hm2->sserial.instance[count]; inst->index = i; inst->num_channels = chan_counts[i]; @@ -815,9 +819,8 @@ int hm2_sserial_parse_md(hostmot2_t *hm2, int md_index){ HM2_ERR("Failed to restart device %i on instance\n", inst->device_id); goto fail0;} - if ((r = hm2_sserial_check_local_errors(hm2, inst)) < 0) { - //goto fail0; // Ignore it for the moment. - } + // Ignore startup errors here; the realtime path tracks them. + (void)hm2_sserial_check_local_errors(hm2, inst); //only increment the instance index if this one is populated //otherwise the "slot" is re-used to keep active ports //contiguous in the array @@ -825,6 +828,16 @@ int hm2_sserial_parse_md(hostmot2_t *hm2, int md_index){ } } + // Unprobed physical ports must not be claimed by hm2_configure_pins(). + for (pin = 0; pin < hm2->num_pins; pin++) { + if ((hm2->pin[pin].sec_tag == HM2_GTAG_SMARTSERIAL + || hm2->pin[pin].sec_tag == HM2_GTAG_SMARTSERIALB) + && hm2->pin[pin].sec_unit >= i + && hm2->pin[pin].sec_unit < md->instances) { + hm2->pin[pin].sec_tag = 0; + } + } + hm2->sserial.num_instances = count; // because of the extra increment // Stop the sserial ports. @@ -1043,7 +1056,6 @@ int hm2_sserial_read_configs(hostmot2_t *hm2, hm2_sserial_remote_t *chan){ ptoc=(buff & 0xffff); if (ptoc == 0) {return chan->num_confs;} // Old 8i20 or 7i64 - c = m = 0; chan->num_confs = 0; do { addr = 0; @@ -1054,11 +1066,12 @@ int hm2_sserial_read_configs(hostmot2_t *hm2, hm2_sserial_remote_t *chan){ } if (rectype == LBP_DATA) { + hm2_sserial_data_t *confs = rtapi_krealloc(chan->confs, + (chan->num_confs + 1) * sizeof(*confs), + RTAPI_GFP_KERNEL); + if (confs == NULL) return -ENOMEM; + chan->confs = confs; c = chan->num_confs++; - chan->confs = (hm2_sserial_data_t *) - rtapi_krealloc(chan->confs, - chan->num_confs * sizeof(hm2_sserial_data_t), - RTAPI_GFP_KERNEL); addr = hm2_sserial_get_bytes(hm2, chan, &chan->confs[c], addr, 14); if (addr < 0){ return -EINVAL;} addr = hm2_sserial_get_bytes(hm2, chan, @@ -1081,12 +1094,12 @@ int hm2_sserial_read_configs(hostmot2_t *hm2, hm2_sserial_remote_t *chan){ HM2_DBG("Process: %s RecordType: %02X Datatype: %02X Dir: %02X Addr: %04X Length: %i\n", chan->confs[c].NameString, chan->confs[c].RecordType,chan->confs[c].DataType, chan->confs[c].DataDir, chan->confs[c].ParmAddr, chan->confs[c].DataLength); } else if (rectype == LBP_MODE ) { - chan->num_modes++; - m = chan->num_modes - 1; - chan->modes = (hm2_sserial_mode_t *) - rtapi_krealloc(chan->modes, - chan->num_modes * sizeof(hm2_sserial_mode_t), - RTAPI_GFP_KERNEL); + hm2_sserial_mode_t *modes = rtapi_krealloc(chan->modes, + (chan->num_modes + 1) * sizeof(*modes), + RTAPI_GFP_KERNEL); + if (modes == NULL) return -ENOMEM; + chan->modes = modes; + m = chan->num_modes++; addr = hm2_sserial_get_bytes(hm2, chan, &chan->modes[m], addr, 4); if (addr < 0){ return -EINVAL;} addr = hm2_sserial_get_bytes(hm2, chan, @@ -1387,11 +1400,9 @@ int hm2_sserial_register_tram(hostmot2_t *hm2, hm2_sserial_remote_t *chan){ } int hm2_sserial_update_params(hostmot2_t *hm2, hm2_sserial_instance_t *inst, long period){ - // init this here to silence a compiler warning - hm2_sserial_remote_t *r = &(inst->remotes[0]); + hm2_sserial_remote_t *r; hm2_sserial_params_t *p; hm2_sserial_data_t *g; - int shift; // used for floating point comparisons switch (hal_get_ui32(inst->state2)){ case 0: // init loop counters @@ -1428,27 +1439,30 @@ int hm2_sserial_register_tram(hostmot2_t *hm2, hm2_sserial_remote_t *chan){ if (hal_get_ui32(p->param.u) != p->u32_written) break; return hal_set_ui32(inst->state2, 2); // increment indices case LBP_FLOAT: - case LBP_NONVOL_FLOAT: - // comparing floats that might have different sizes is not trivial - // this does a bitwise comparison of as many mantissa bits as might - // be expected to have been sent by the sserial remote + case LBP_NONVOL_FLOAT: { + double value = hal_get_real(p->param.r); + double written = p->float_written; + // Parameter read/write supports IEEE binary32 and + // binary64 only; 8/16-bit float formats are not + // decoded by hm2_sserial_get_param_value(). switch (g->DataLength){ - // ( double significand - variable type significand) - default: - HM2_ERR("Non IEEE float type parameter of length %i\n", g->DataLength); - /* Fallthrough */ - case 8: - shift = (52 - 4); break; // 1.3.4 minifloat, if we ever add them - case 16: - shift = (52 - 10); break; case 32: - shift = (52 - 23); break; + value = (float)value; + written = (float)written; + /* Fallthrough */ case 64: - shift = 0; - } - // FIXME: This overlayed s64 read is very wrong! - if (xlabs((hal_get_sint(p->param.s) - p->s64_written) >> shift) > 2) break; - return hal_set_ui32(inst->state2, 2); // increment indices + // An unchanged NaN must not repeatedly stop + // the port and rewrite the parameter. + if (value != written + && !(isnan(value) && isnan(written))) break; + return hal_set_ui32(inst->state2, 2); + default: + HM2_ERR("Non IEEE float type parameter of length %i\n", g->DataLength); + p->type = LBP_PAD; // warn once, then ignore + return hal_set_ui32(inst->state2, 2); + } + break; + } default: return hal_set_ui32(inst->state2, 2); // increment indices } @@ -1665,7 +1679,7 @@ void hm2_sserial_write_pins(hostmot2_t *hm2, hm2_sserial_instance_t *inst){ "if this is happening frequently.\n", inst->index, hm2->llio->name, inst->index); } - fault_count = hal_set_ui32(inst->fault_count, fault_count + hal_get_ui32(inst->fault_inc)); + hal_set_ui32(inst->fault_count, fault_count + hal_get_ui32(inst->fault_inc)); *inst->command_reg_write = 0x80000000; // set bit31 for ignored cmd return; // give the register chance to clear } @@ -1674,11 +1688,11 @@ void hm2_sserial_write_pins(hostmot2_t *hm2, hm2_sserial_instance_t *inst){ } if (fault_count > hal_get_ui32(inst->fault_dec)) { - fault_count = hal_set_ui32(inst->fault_count, fault_count - hal_get_ui32(inst->fault_dec)); + hal_set_ui32(inst->fault_count, fault_count - hal_get_ui32(inst->fault_dec)); } else { - fault_count = hal_set_ui32(inst->fault_count, 0); + hal_set_ui32(inst->fault_count, 0); } // All seems well, handle the pins. @@ -1696,6 +1710,7 @@ void hm2_sserial_write_pins(hostmot2_t *hm2, hm2_sserial_instance_t *inst){ hm2_sserial_data_t *conf = &chan->confs[p]; hm2_sserial_pins_t *pin = &chan->pins[p]; if (conf->DataDir & 0xC0){ + buff = 0; switch (conf->DataType){ case LBP_PAD: // do nothing @@ -2185,24 +2200,37 @@ void hm2_sserial_force_write(hostmot2_t *hm2){ void hm2_sserial_cleanup(hostmot2_t *hm2){ int i,r; rtapi_u32 buff; - for (i = 1 ; i < hm2->sserial.num_instances; i++){ + if (hm2->sserial.instance == NULL) return; + + for (i = 0 ; i < hm2->sserial.num_instances; i++){ //Shut down the sserial devices rather than leave that to the watchdog. - buff = 0x800; - hm2->llio->write(hm2->llio, - hm2->sserial.instance[i].command_reg_addr, - &buff, - sizeof(rtapi_u32)); + if (hm2->sserial.instance[i].command_reg_addr != 0) { + buff = 0x800; + hm2->llio->write(hm2->llio, + hm2->sserial.instance[i].command_reg_addr, + &buff, + sizeof(rtapi_u32)); + } if (hm2->sserial.instance[i].remotes != NULL){ for (r = 0 ; r < hm2->sserial.instance[i].num_remotes; r++){ - if (hm2->sserial.instance[i].remotes[r].num_confs > 0){ - rtapi_kfree(hm2->sserial.instance[i].remotes[r].confs); - }; - if (hm2->sserial.instance[i].remotes[r].num_modes > 0){ - rtapi_kfree(hm2->sserial.instance[i].remotes[r].modes); - } + hm2_sserial_remote_t *remote = + &hm2->sserial.instance[i].remotes[r]; + + rtapi_kfree(remote->confs); + remote->confs = NULL; + remote->num_confs = 0; + rtapi_kfree(remote->modes); + remote->modes = NULL; + remote->num_modes = 0; + rtapi_kfree(remote->globals); + remote->globals = NULL; + remote->num_globals = 0; } rtapi_kfree(hm2->sserial.instance[i].remotes); + hm2->sserial.instance[i].remotes = NULL; } + hm2->sserial.instance[i].num_remotes = 0; } + hm2->sserial.num_instances = 0; }