/**
* - Move to the given XY
* - Deploy the probe, if not already deployed
* - Probe the bed, get the Z position
* - Depending on the 'stow' flag
* - Stow the probe, or
* - Raise to the BETWEEN height
* - Return the probed Z position
*/
float probe_pt(const float &lx, const float &ly, const bool stow, const uint8_t verbose_level, const bool printable/*=true*/) {
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
SERIAL_ECHOPAIR(">>> probe_pt(", lx);
SERIAL_ECHOPAIR(", ", ly);
SERIAL_ECHOPAIR(", ", stow ? "" : "no ");
SERIAL_ECHOLNPGM("stow)");
DEBUG_POS("", current_position);
}
#endif
const float nx = lx - (X_PROBE_OFFSET_FROM_EXTRUDER), ny = ly - (Y_PROBE_OFFSET_FROM_EXTRUDER);
if (printable
? !position_is_reachable_xy(nx, ny)
: !position_is_reachable_by_probe_xy(lx, ly)
) return NAN;
const float old_feedrate_mm_s = feedrate_mm_s;
#if ENABLED(DELTA)
if (current_position[Z_AXIS] > delta_clip_start_height)
do_blocking_move_to_z(delta_clip_start_height);
#endif
#if HAS_SOFTWARE_ENDSTOPS
// Store the status of the soft endstops and disable if we're probing a non-printable location
static bool enable_soft_endstops = soft_endstops_enabled;
if (!printable) soft_endstops_enabled = false;
#endif
feedrate_mm_s = XY_PROBE_FEEDRATE_MM_S;
// Move the probe to the given XY
do_blocking_move_to_xy(nx, ny);
float measured_z = NAN;
if (!DEPLOY_PROBE()) {
measured_z = run_z_probe(printable);
if (!stow)
do_blocking_move_to_z(current_position[Z_AXIS] + Z_CLEARANCE_BETWEEN_PROBES, MMM_TO_MMS(Z_PROBE_SPEED_FAST));
else
if (STOW_PROBE()) measured_z = NAN;
}
#if HAS_SOFTWARE_ENDSTOPS
// Restore the soft endstop status
soft_endstops_enabled = enable_soft_endstops;
#endif
if (verbose_level > 2) {
SERIAL_PROTOCOLPGM("Bed X: ");
SERIAL_PROTOCOL_F(lx, 3);
SERIAL_PROTOCOLPGM(" Y: ");
SERIAL_PROTOCOL_F(ly, 3);
SERIAL_PROTOCOLPGM(" Z: ");
SERIAL_PROTOCOL_F(measured_z, 3);
SERIAL_EOL();
}
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< probe_pt");
#endif
feedrate_mm_s = old_feedrate_mm_s;
if (isnan(measured_z)) {
LCD_MESSAGEPGM(MSG_ERR_PROBING_FAILED);
SERIAL_ERROR_START();
SERIAL_ERRORLNPGM(MSG_ERR_PROBING_FAILED);
}
return measured_z;
}
void Stopwatch::debug(const char func[]) {
if (DEBUGGING(INFO)) {
ECHO_MT("Stopwatch::", func);
ECHO_EM("()");
}
}
DEBUG_NO_STATIC int
pfkey_supported_parse(struct sadb_ext *pfkey_ext)
{
int error = 0;
unsigned int i, num_alg;
struct sadb_supported *pfkey_supported = (struct sadb_supported *)pfkey_ext;
struct sadb_alg *pfkey_alg = (struct sadb_alg*)((char*)pfkey_ext + sizeof(struct sadb_supported));
/* sanity checks... */
if((pfkey_supported->sadb_supported_len <
sizeof(struct sadb_supported) / IPSEC_PFKEYv2_ALIGN) ||
(((pfkey_supported->sadb_supported_len * IPSEC_PFKEYv2_ALIGN) -
sizeof(struct sadb_supported)) % sizeof(struct sadb_alg))) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_supported_parse: "
"size wrong ext_len=%d, supported_ext_len=%ld alg_ext_len=%ld.\n",
pfkey_supported->sadb_supported_len,
sizeof(struct sadb_supported),
sizeof(struct sadb_alg));
SENDERR(EINVAL);
}
if(pfkey_supported->sadb_supported_reserved) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_supported_parse: "
"res=%d, must be zero.\n",
pfkey_supported->sadb_supported_reserved);
SENDERR(EINVAL);
}
num_alg = ((pfkey_supported->sadb_supported_len * IPSEC_PFKEYv2_ALIGN) - sizeof(struct sadb_supported)) / sizeof(struct sadb_alg);
for(i = 0; i < num_alg; i++) {
/* process algo description */
if(pfkey_alg->sadb_alg_reserved) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_supported_parse: "
"alg[%d], id=%d, ivlen=%d, minbits=%d, maxbits=%d, res=%d, must be zero.\n",
i,
pfkey_alg->sadb_alg_id,
pfkey_alg->sadb_alg_ivlen,
pfkey_alg->sadb_alg_minbits,
pfkey_alg->sadb_alg_maxbits,
pfkey_alg->sadb_alg_reserved);
SENDERR(EINVAL);
}
/* XXX can alg_id auth/enc be determined from info given?
Yes, but OpenBSD's method does not iteroperate with rfc2367.
rgb, 2000-04-06 */
switch(pfkey_supported->sadb_supported_exttype) {
case SADB_EXT_SUPPORTED_AUTH:
if(pfkey_alg->sadb_alg_id > SADB_AALG_MAX) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_supported_parse: "
"alg[%d], alg_id=%d > SADB_AALG_MAX=%d, fatal.\n",
i,
pfkey_alg->sadb_alg_id,
SADB_AALG_MAX);
SENDERR(EINVAL);
}
break;
case SADB_EXT_SUPPORTED_ENCRYPT:
if(pfkey_alg->sadb_alg_id > SADB_EALG_MAX) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_supported_parse: "
"alg[%d], alg_id=%d > SADB_EALG_MAX=%d, fatal.\n",
i,
pfkey_alg->sadb_alg_id,
SADB_EALG_MAX);
SENDERR(EINVAL);
}
break;
default:
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_supported_parse: "
"alg[%d], alg_id=%d > SADB_EALG_MAX=%d, fatal.\n",
i,
pfkey_alg->sadb_alg_id,
SADB_EALG_MAX);
SENDERR(EINVAL);
}
pfkey_alg++;
}
errlab:
return error;
}
// returns false for ok and true for failure
bool set_probe_deployed(const bool deploy) {
// Can be extended to servo probes, if needed.
#if ENABLED(PROBE_IS_TRIGGERED_WHEN_STOWED_TEST)
#if ENABLED(Z_MIN_PROBE_ENDSTOP)
#define _TRIGGERED_WHEN_STOWED_TEST (READ(Z_MIN_PROBE_PIN) != Z_MIN_PROBE_ENDSTOP_INVERTING)
#else
#define _TRIGGERED_WHEN_STOWED_TEST (READ(Z_MIN_PIN) != Z_MIN_ENDSTOP_INVERTING)
#endif
#endif
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
DEBUG_POS("set_probe_deployed", current_position);
SERIAL_ECHOLNPAIR("deploy: ", deploy);
}
#endif
if (endstops.z_probe_enabled == deploy) return false;
// Make room for probe
do_probe_raise(_Z_CLEARANCE_DEPLOY_PROBE);
#if ENABLED(Z_PROBE_SLED) || ENABLED(Z_PROBE_ALLEN_KEY)
#if ENABLED(Z_PROBE_SLED)
#define _AUE_ARGS true, false, false
#else
#define _AUE_ARGS
#endif
if (axis_unhomed_error(_AUE_ARGS)) {
SERIAL_ERROR_START();
SERIAL_ERRORLNPGM(MSG_STOP_UNHOMED);
stop();
return true;
}
#endif
const float oldXpos = current_position[X_AXIS],
oldYpos = current_position[Y_AXIS];
#ifdef _TRIGGERED_WHEN_STOWED_TEST
// If endstop is already false, the Z probe is deployed
if (_TRIGGERED_WHEN_STOWED_TEST == deploy) { // closed after the probe specific actions.
// Would a goto be less ugly?
//while (!_TRIGGERED_WHEN_STOWED_TEST) idle(); // would offer the opportunity
// for a triggered when stowed manual probe.
if (!deploy) endstops.enable_z_probe(false); // Switch off triggered when stowed probes early
// otherwise an Allen-Key probe can't be stowed.
#endif
#if ENABLED(SOLENOID_PROBE)
#if HAS_SOLENOID_1
WRITE(SOL1_PIN, deploy);
#endif
#elif ENABLED(Z_PROBE_SLED)
dock_sled(!deploy);
#elif HAS_Z_SERVO_ENDSTOP && DISABLED(BLTOUCH)
MOVE_SERVO(Z_ENDSTOP_SERVO_NR, z_servo_angle[deploy ? 0 : 1]);
#elif ENABLED(Z_PROBE_ALLEN_KEY)
deploy ? run_deploy_moves_script() : run_stow_moves_script();
#endif
#ifdef _TRIGGERED_WHEN_STOWED_TEST
} // _TRIGGERED_WHEN_STOWED_TEST == deploy
if (_TRIGGERED_WHEN_STOWED_TEST == deploy) { // State hasn't changed?
if (IsRunning()) {
SERIAL_ERROR_START();
SERIAL_ERRORLNPGM("Z-Probe failed");
LCD_ALERTMESSAGEPGM("Err: ZPROBE");
}
stop();
return true;
} // _TRIGGERED_WHEN_STOWED_TEST == deploy
#endif
do_blocking_move_to(oldXpos, oldYpos, current_position[Z_AXIS]); // return to position before deploy
endstops.enable_z_probe(deploy);
return false;
}
/**
* Perform a tool-change, which may result in moving the
* previous tool out of the way and the new tool into place.
*/
void tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool no_move/*=false*/) {
#if ENABLED(MIXING_EXTRUDER)
UNUSED(fr_mm_s); UNUSED(no_move);
if (tmp_extruder >= MIXING_VIRTUAL_TOOLS)
return invalid_extruder_error(tmp_extruder);
#if MIXING_VIRTUAL_TOOLS > 1
// T0-Tnnn: Switch virtual tool by changing the index to the mix
mixer.T(tmp_extruder);
#endif
#elif ENABLED(PRUSA_MMU2)
UNUSED(fr_mm_s); UNUSED(no_move);
mmu2.toolChange(tmp_extruder);
#elif EXTRUDERS < 2
UNUSED(fr_mm_s); UNUSED(no_move);
if (tmp_extruder) invalid_extruder_error(tmp_extruder);
return;
#else // EXTRUDERS > 1
planner.synchronize();
#if ENABLED(DUAL_X_CARRIAGE) // Only T0 allowed if the Printer is in DXC_DUPLICATION_MODE or DXC_MIRRORED_MODE
if (tmp_extruder != 0 && dxc_is_duplicating())
return invalid_extruder_error(tmp_extruder);
#endif
#if HAS_LEVELING
// Set current position to the physical position
const bool leveling_was_active = planner.leveling_active;
set_bed_leveling_enabled(false);
#endif
if (tmp_extruder >= EXTRUDERS)
return invalid_extruder_error(tmp_extruder);
if (!no_move && !all_axes_homed()) {
no_move = true;
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("No move on toolchange");
}
#if HAS_LCD_MENU
ui.return_to_status();
#endif
#if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
const bool should_swap = !no_move && toolchange_settings.swap_length;
#if ENABLED(PREVENT_COLD_EXTRUSION)
const bool too_cold = !DEBUGGING(DRYRUN) && (thermalManager.targetTooColdToExtrude(active_extruder) || thermalManager.targetTooColdToExtrude(tmp_extruder));
#else
constexpr bool too_cold = false;
#endif
if (should_swap) {
if (too_cold) {
SERIAL_ECHO_MSG(MSG_ERR_HOTEND_TOO_COLD);
#if ENABLED(SINGLENOZZLE)
active_extruder = tmp_extruder;
return;
#endif
}
else {
#if ENABLED(ADVANCED_PAUSE_FEATURE)
do_pause_e_move(-toolchange_settings.swap_length, MMM_TO_MMS(toolchange_settings.retract_speed));
#else
current_position[E_AXIS] -= toolchange_settings.swap_length / planner.e_factor[active_extruder];
planner.buffer_line(current_position, MMM_TO_MMS(toolchange_settings.retract_speed), active_extruder);
#endif
}
}
#endif // TOOLCHANGE_FILAMENT_SWAP
if (tmp_extruder != active_extruder) {
#if SWITCHING_NOZZLE_TWO_SERVOS
raise_nozzle(active_extruder);
#endif
const float old_feedrate_mm_s = fr_mm_s > 0.0 ? fr_mm_s : feedrate_mm_s;
feedrate_mm_s = fr_mm_s > 0.0 ? fr_mm_s : XY_PROBE_FEEDRATE_MM_S;
#if HAS_SOFTWARE_ENDSTOPS && ENABLED(DUAL_X_CARRIAGE)
update_software_endstops(X_AXIS, active_extruder, tmp_extruder);
#endif
set_destination_from_current();
if (!no_move) {
#if DISABLED(SWITCHING_NOZZLE)
// Do a small lift to avoid the workpiece in the move back (below)
#if ENABLED(TOOLCHANGE_PARK)
current_position[X_AXIS] = toolchange_settings.change_point.x;
current_position[Y_AXIS] = toolchange_settings.change_point.y;
#endif
current_position[Z_AXIS] += toolchange_settings.z_raise;
#if HAS_SOFTWARE_ENDSTOPS
NOMORE(current_position[Z_AXIS], soft_endstop[Z_AXIS].max);
#endif
planner.buffer_line(current_position, feedrate_mm_s, active_extruder);
#endif
planner.synchronize();
}
#if HAS_HOTEND_OFFSET
#if ENABLED(DUAL_X_CARRIAGE)
constexpr float xdiff = 0;
#else
const float xdiff = hotend_offset[X_AXIS][tmp_extruder] - hotend_offset[X_AXIS][active_extruder];
#endif
const float ydiff = hotend_offset[Y_AXIS][tmp_extruder] - hotend_offset[Y_AXIS][active_extruder],
zdiff = hotend_offset[Z_AXIS][tmp_extruder] - hotend_offset[Z_AXIS][active_extruder];
#else
constexpr float xdiff = 0, ydiff = 0, zdiff = 0;
#endif
#if ENABLED(DUAL_X_CARRIAGE)
dualx_tool_change(tmp_extruder, no_move);
#elif ENABLED(PARKING_EXTRUDER) // Dual Parking extruder
parking_extruder_tool_change(tmp_extruder, no_move);
#elif ENABLED(MAGNETIC_PARKING_EXTRUDER) // Magnetic Parking extruder
magnetic_parking_extruder_tool_change(tmp_extruder);
#elif ENABLED(SWITCHING_TOOLHEAD) // Switching Toolhead
switching_toolhead_tool_change(tmp_extruder, fr_mm_s, no_move);
#elif ENABLED(SWITCHING_NOZZLE) && !SWITCHING_NOZZLE_TWO_SERVOS
// Raise by a configured distance to avoid workpiece, except with
// SWITCHING_NOZZLE_TWO_SERVOS, as both nozzles will lift instead.
current_position[Z_AXIS] += MAX(-zdiff, 0.0) + toolchange_settings.z_raise;
#if HAS_SOFTWARE_ENDSTOPS
NOMORE(current_position[Z_AXIS], soft_endstop[Z_AXIS].max);
#endif
if (!no_move) fast_line_to_current(Z_AXIS);
move_nozzle_servo(tmp_extruder);
#endif
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPAIR("Offset Tool XY by { ", xdiff, ", ", ydiff, ", ", zdiff, " }");
// The newly-selected extruder XY is actually at...
current_position[X_AXIS] += xdiff;
current_position[Y_AXIS] += ydiff;
current_position[Z_AXIS] += zdiff;
// Set the new active extruder if not already done in tool specific function above
active_extruder = tmp_extruder;
// Tell the planner the new "current position"
sync_plan_position();
#if ENABLED(DELTA)
//LOOP_XYZ(i) update_software_endstops(i); // or modify the constrain function
const bool safe_to_move = current_position[Z_AXIS] < delta_clip_start_height - 1;
#else
constexpr bool safe_to_move = true;
#endif
// Return to position and lower again
if (safe_to_move && !no_move && IsRunning()) {
if (DEBUGGING(LEVELING)) DEBUG_POS("Move back", destination);
#if ENABLED(SINGLENOZZLE)
#if FAN_COUNT > 0
singlenozzle_fan_speed[active_extruder] = thermalManager.fan_speed[0];
thermalManager.fan_speed[0] = singlenozzle_fan_speed[tmp_extruder];
#endif
singlenozzle_temp[active_extruder] = thermalManager.temp_hotend[0].target;
if (singlenozzle_temp[tmp_extruder] && singlenozzle_temp[tmp_extruder] != singlenozzle_temp[active_extruder]) {
thermalManager.setTargetHotend(singlenozzle_temp[tmp_extruder], 0);
#if EITHER(ULTRA_LCD, EXTENSIBLE_UI)
thermalManager.set_heating_message(0);
#endif
(void)thermalManager.wait_for_hotend(0, false); // Wait for heating or cooling
}
active_extruder = tmp_extruder;
#endif
#if ENABLED(TOOLCHANGE_FILAMENT_SWAP)
if (should_swap && !too_cold) {
#if ENABLED(ADVANCED_PAUSE_FEATURE)
do_pause_e_move(toolchange_settings.swap_length + TOOLCHANGE_FIL_EXTRA_PRIME, toolchange_settings.prime_speed);
#else
current_position[E_AXIS] += (toolchange_settings.swap_length + TOOLCHANGE_FIL_EXTRA_PRIME) / planner.e_factor[tmp_extruder];
planner.buffer_line(current_position, toolchange_settings.prime_speed, tmp_extruder);
#endif
planner.synchronize();
#if TOOLCHANGE_FIL_EXTRA_PRIME
planner.set_e_position_mm((destination[E_AXIS] = current_position[E_AXIS] = current_position[E_AXIS] - (TOOLCHANGE_FIL_EXTRA_PRIME)));
#endif
}
#endif
// Prevent a move outside physical bounds
apply_motion_limits(destination);
// Move back to the original (or tweaked) position
do_blocking_move_to(destination);
#if ENABLED(DUAL_X_CARRIAGE)
active_extruder_parked = false;
#endif
feedrate_mm_s = old_feedrate_mm_s;
}
#if ENABLED(SWITCHING_NOZZLE)
else {
// Move back down. (Including when the new tool is higher.)
do_blocking_move_to_z(destination[Z_AXIS], planner.settings.max_feedrate_mm_s[Z_AXIS]);
}
#endif
#if ENABLED(PRUSA_MMU2)
mmu2.toolChange(tmp_extruder);
#endif
#if SWITCHING_NOZZLE_TWO_SERVOS
lower_nozzle(active_extruder);
#endif
#if ENABLED(TOOLCHANGE_FILAMENT_SWAP) && ADVANCED_PAUSE_RESUME_PRIME != 0
if (should_swap && !too_cold) {
const float resume_eaxis = current_position[E_AXIS];
#if ENABLED(ADVANCED_PAUSE_FEATURE)
do_pause_e_move(toolchange_settings.swap_length, toolchange_settings.prime_speed);
#else
current_position[E_AXIS] += (ADVANCED_PAUSE_RESUME_PRIME) / planner.e_factor[active_extruder];
planner.buffer_line(current_position, ADVANCED_PAUSE_PURGE_FEEDRATE, active_extruder);
#endif
planner.synchronize();
planner.set_e_position_mm((destination[E_AXIS] = current_position[E_AXIS] = resume_eaxis));
}
#endif
} // (tmp_extruder != active_extruder)
planner.synchronize();
#if ENABLED(EXT_SOLENOID) && DISABLED(PARKING_EXTRUDER)
disable_all_solenoids();
enable_solenoid_on_active_extruder();
#endif
#if ENABLED(MK2_MULTIPLEXER)
if (tmp_extruder >= E_STEPPERS) return invalid_extruder_error(tmp_extruder);
select_multiplexed_stepper(tmp_extruder);
#endif
#if DO_SWITCH_EXTRUDER
planner.synchronize();
move_extruder_servo(active_extruder);
#endif
#if HAS_FANMUX
fanmux_switch(active_extruder);
#endif
#if HAS_LEVELING
// Restore leveling to re-establish the logical position
set_bed_leveling_enabled(leveling_was_active);
#endif
SERIAL_ECHO_START();
SERIAL_ECHOLNPAIR(MSG_ACTIVE_EXTRUDER, int(active_extruder));
#endif // EXTRUDERS > 1
}
static int load_one_glyph(DviContext *dvi, DviFont *font, int code)
{
BITMAP *map;
DviFontChar *ch;
int status;
#ifndef NODEBUG
ch = FONTCHAR(font, code);
DEBUG((DBG_GLYPHS, "loading glyph code %d in %s (at %u)\n",
code, font->fontname, ch->offset));
#endif
if(font->finfo->getglyph == NULL) {
/* font type does not need to load glyphs (e.g. vf) */
return 0;
}
status = font->finfo->getglyph(&dvi->params, font, code);
if(status < 0)
return -1;
/* get the glyph again (font->chars may have changed) */
ch = FONTCHAR(font, code);
#ifndef NODEBUG
map = (BITMAP *)ch->glyph.data;
if(DEBUGGING(BITMAP_DATA)) {
DEBUG((DBG_BITMAP_DATA,
"%s: new %s bitmap for character %d:\n",
font->fontname, TYPENAME(font), code));
if(MDVI_GLYPH_ISEMPTY(map))
DEBUG((DBG_BITMAP_DATA, "blank bitmap\n"));
else
bitmap_print(stderr, map);
}
#endif
/* check if we have to scale it */
if(!font->finfo->scalable && font->hdpi != font->vdpi) {
int hs, vs, d;
/* we scale it ourselves */
d = Max(font->hdpi, font->vdpi);
hs = d / font->hdpi;
vs = d / font->vdpi;
if(ch->width && ch->height && (hs > 1 || vs > 1)) {
int h, v;
DviGlyph glyph;
DEBUG((DBG_FONTS,
"%s: scaling glyph %d to resolution %dx%d\n",
font->fontname, code, font->hdpi, font->vdpi));
h = dvi->params.hshrink;
v = dvi->params.vshrink;
d = dvi->params.density;
dvi->params.hshrink = hs;
dvi->params.vshrink = vs;
dvi->params.density = 50;
/* shrink it */
font->finfo->shrink0(dvi, font, ch, &glyph);
/* restore parameters */
dvi->params.hshrink = h;
dvi->params.vshrink = v;
dvi->params.density = d;
/* update glyph data */
if(!MDVI_GLYPH_ISEMPTY(ch->glyph.data))
bitmap_destroy((BITMAP *)ch->glyph.data);
ch->glyph.data = glyph.data;
ch->glyph.x = glyph.x;
ch->glyph.y = glyph.y;
ch->glyph.w = glyph.w;
ch->glyph.h = glyph.h;
}
}
font_transform_glyph(dvi->params.orientation, &ch->glyph);
return 0;
}
DEBUG_NO_STATIC int
pfkey_sa_parse(struct sadb_ext *pfkey_ext)
{
int error = 0;
struct sadb_sa *pfkey_sa = (struct sadb_sa *)pfkey_ext;
DEBUGGING(PF_KEY_DEBUG_PARSE_FLOW,
"pfkey_sa_parse: entry\n");
/* sanity checks... */
if(!pfkey_sa) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_sa_parse: "
"NULL pointer passed in.\n");
SENDERR(EINVAL);
}
if(pfkey_sa->sadb_sa_len != sizeof(struct sadb_sa) / IPSEC_PFKEYv2_ALIGN) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_sa_parse: "
"length wrong pfkey_sa->sadb_sa_len=%d sizeof(struct sadb_sa)=%ld.\n",
pfkey_sa->sadb_sa_len,
sizeof(struct sadb_sa));
SENDERR(EINVAL);
}
if(pfkey_sa->sadb_sa_encrypt > SADB_EALG_MAX) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_sa_parse: "
"pfkey_sa->sadb_sa_encrypt=%d > SADB_EALG_MAX=%d.\n",
pfkey_sa->sadb_sa_encrypt,
SADB_EALG_MAX);
SENDERR(EINVAL);
}
if(pfkey_sa->sadb_sa_auth > SADB_AALG_MAX) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_sa_parse: "
"pfkey_sa->sadb_sa_auth=%d > SADB_AALG_MAX=%d.\n",
pfkey_sa->sadb_sa_auth,
SADB_AALG_MAX);
SENDERR(EINVAL);
}
if(pfkey_sa->sadb_sa_state > SADB_SASTATE_MAX) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_sa_parse: "
"state=%d exceeds MAX=%d.\n",
pfkey_sa->sadb_sa_state,
SADB_SASTATE_MAX);
SENDERR(EINVAL);
}
if(pfkey_sa->sadb_sa_state == SADB_SASTATE_DEAD) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_sa_parse: "
"state=%d is DEAD=%d.\n",
pfkey_sa->sadb_sa_state,
SADB_SASTATE_DEAD);
SENDERR(EINVAL);
}
if(pfkey_sa->sadb_sa_replay > 64) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_sa_parse: "
"replay window size: %d -- must be 0 <= size <= 64\n",
pfkey_sa->sadb_sa_replay);
SENDERR(EINVAL);
}
if(! ((pfkey_sa->sadb_sa_exttype == SADB_EXT_SA) ||
(pfkey_sa->sadb_sa_exttype == SADB_X_EXT_SA2)))
{
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_sa_parse: "
"unknown exttype=%d, expecting SADB_EXT_SA=%d or SADB_X_EXT_SA2=%d.\n",
pfkey_sa->sadb_sa_exttype,
SADB_EXT_SA,
SADB_X_EXT_SA2);
SENDERR(EINVAL);
}
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_sa_parse: "
"successfully found len=%d exttype=%d(%s) spi=%08lx replay=%d state=%d auth=%d encrypt=%d flags=%d.\n",
pfkey_sa->sadb_sa_len,
pfkey_sa->sadb_sa_exttype, pfkey_v2_sadb_ext_string(pfkey_sa->sadb_sa_exttype),
(long unsigned int)ntohl(pfkey_sa->sadb_sa_spi),
pfkey_sa->sadb_sa_replay,
pfkey_sa->sadb_sa_state,
pfkey_sa->sadb_sa_auth,
pfkey_sa->sadb_sa_encrypt,
pfkey_sa->sadb_sa_flags);
errlab:
return error;
}
inline void parking_extruder_tool_change(const uint8_t tmp_extruder, bool no_move) {
if (!no_move) {
constexpr float parkingposx[] = PARKING_EXTRUDER_PARKING_X;
#if HAS_HOTEND_OFFSET
const float x_offset = hotend_offset[X_AXIS][active_extruder];
#else
constexpr float x_offset = 0;
#endif
const float midpos = (parkingposx[0] + parkingposx[1]) * 0.5 + x_offset,
grabpos = parkingposx[tmp_extruder] + (tmp_extruder ? PARKING_EXTRUDER_GRAB_DISTANCE : -(PARKING_EXTRUDER_GRAB_DISTANCE)) + x_offset;
/**
* 1. Raise Z-Axis to give enough clearance
* 2. Move to park position of old extruder
* 3. Disengage magnetic field, wait for delay
* 4. Move near new extruder
* 5. Engage magnetic field for new extruder
* 6. Move to parking incl. offset of new extruder
* 7. Lower Z-Axis
*/
// STEP 1
if (DEBUGGING(LEVELING)) DEBUG_POS("Start Autopark", current_position);
current_position[Z_AXIS] += toolchange_settings.z_raise;
if (DEBUGGING(LEVELING)) DEBUG_POS("(1) Raise Z-Axis", current_position);
fast_line_to_current(Z_AXIS);
planner.synchronize();
// STEP 2
current_position[X_AXIS] = parkingposx[active_extruder] + x_offset;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOLNPAIR("(2) Park extruder ", int(active_extruder));
DEBUG_POS("Moving ParkPos", current_position);
}
fast_line_to_current(X_AXIS);
planner.synchronize();
// STEP 3
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(3) Disengage magnet ");
pe_deactivate_solenoid(active_extruder);
// STEP 4
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(4) Move to position near new extruder");
current_position[X_AXIS] += active_extruder ? -10 : 10; // move 10mm away from parked extruder
if (DEBUGGING(LEVELING)) DEBUG_POS("Move away from parked extruder", current_position);
fast_line_to_current(X_AXIS);
planner.synchronize();
// STEP 5
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(5) Engage magnetic field");
#if ENABLED(PARKING_EXTRUDER_SOLENOIDS_INVERT)
pe_activate_solenoid(active_extruder); //just save power for inverted magnets
#endif
pe_activate_solenoid(tmp_extruder);
// STEP 6
current_position[X_AXIS] = grabpos + (tmp_extruder ? -10 : 10);
fast_line_to_current(X_AXIS);
current_position[X_AXIS] = grabpos;
if (DEBUGGING(LEVELING)) DEBUG_POS("(6) Unpark extruder", current_position);
planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[X_AXIS] * 0.5, active_extruder);
planner.synchronize();
// STEP 7
current_position[X_AXIS] = midpos
#if HAS_HOTEND_OFFSET
- hotend_offset[X_AXIS][tmp_extruder]
#endif
;
if (DEBUGGING(LEVELING)) DEBUG_POS("(7) Move midway between hotends", current_position);
fast_line_to_current(X_AXIS);
planner.synchronize();
DEBUG_ECHOLNPGM("Autopark done.");
}
else { // nomove == true
// Only engage magnetic field for new extruder
pe_activate_solenoid(tmp_extruder);
#if ENABLED(PARKING_EXTRUDER_SOLENOIDS_INVERT)
pe_activate_solenoid(active_extruder); // Just save power for inverted magnets
#endif
}
#if HAS_HOTEND_OFFSET
current_position[Z_AXIS] += hotend_offset[Z_AXIS][active_extruder] - hotend_offset[Z_AXIS][tmp_extruder];
#endif
if (DEBUGGING(LEVELING)) DEBUG_POS("Applying Z-offset", current_position);
}
void Planner::buffer_line(const float& x, const float& y, const float& z, const float& e, float feed_rate, const uint8_t extruder)
#endif // AUTO_BED_LEVELING_FEATURE
{
// Calculate the buffer head after we push this byte
int next_buffer_head = next_block_index(block_buffer_head);
// If the buffer is full: good! That means we are well ahead of the robot.
// Rest here until there is room in the buffer.
while (block_buffer_tail == next_buffer_head) idle();
#if ENABLED(MESH_BED_LEVELING)
if (mbl.active) z += mbl.get_z(x - home_offset[X_AXIS], y - home_offset[Y_AXIS]);
#elif ENABLED(AUTO_BED_LEVELING_FEATURE)
apply_rotation_xyz(bed_level_matrix, x, y, z);
#endif
// The target position of the tool in absolute steps
// Calculate target position in absolute steps
//this should be done after the wait, because otherwise a M92 code within the gcode disrupts this calculation somehow
long target[NUM_AXIS] = {
lround(x * axis_steps_per_unit[X_AXIS]),
lround(y * axis_steps_per_unit[Y_AXIS]),
lround(z * axis_steps_per_unit[Z_AXIS]),
lround(e * axis_steps_per_unit[E_AXIS])
};
long dx = target[X_AXIS] - position[X_AXIS],
dy = target[Y_AXIS] - position[Y_AXIS],
dz = target[Z_AXIS] - position[Z_AXIS];
// DRYRUN ignores all temperature constraints and assures that the extruder is instantly satisfied
if (DEBUGGING(DRYRUN))
position[E_AXIS] = target[E_AXIS];
long de = target[E_AXIS] - position[E_AXIS];
#if ENABLED(PREVENT_DANGEROUS_EXTRUDE)
if (de) {
if (thermalManager.tooColdToExtrude(extruder)) {
position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
de = 0; // no difference
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_COLD_EXTRUDE_STOP);
}
#if ENABLED(PREVENT_LENGTHY_EXTRUDE)
if (labs(de) > axis_steps_per_unit[E_AXIS] * (EXTRUDE_MAXLENGTH)) {
position[E_AXIS] = target[E_AXIS]; // Behave as if the move really took place, but ignore E part
de = 0; // no difference
SERIAL_ECHO_START;
SERIAL_ECHOLNPGM(MSG_ERR_LONG_EXTRUDE_STOP);
}
#endif
}
#endif
// Prepare to set up new block
block_t* block = &block_buffer[block_buffer_head];
// Mark block as not busy (Not executed by the stepper interrupt)
block->busy = false;
// Number of steps for each axis
#if ENABLED(COREXY)
// corexy planning
// these equations follow the form of the dA and dB equations on http://www.corexy.com/theory.html
block->steps[A_AXIS] = labs(dx + dy);
block->steps[B_AXIS] = labs(dx - dy);
block->steps[Z_AXIS] = labs(dz);
#elif ENABLED(COREXZ)
// corexz planning
block->steps[A_AXIS] = labs(dx + dz);
block->steps[Y_AXIS] = labs(dy);
block->steps[C_AXIS] = labs(dx - dz);
#else
// default non-h-bot planning
block->steps[X_AXIS] = labs(dx);
block->steps[Y_AXIS] = labs(dy);
block->steps[Z_AXIS] = labs(dz);
#endif
block->steps[E_AXIS] = labs(de);
block->steps[E_AXIS] *= volumetric_multiplier[extruder];
block->steps[E_AXIS] *= extruder_multiplier[extruder];
block->steps[E_AXIS] /= 100;
block->step_event_count = max(block->steps[X_AXIS], max(block->steps[Y_AXIS], max(block->steps[Z_AXIS], block->steps[E_AXIS])));
// Bail if this is a zero-length block
if (block->step_event_count <= dropsegments) return;
#if FAN_COUNT > 0
for (uint8_t i = 0; i < FAN_COUNT; i++) block->fan_speed[i] = fanSpeeds[i];
#endif
#if ENABLED(BARICUDA)
block->valve_pressure = baricuda_valve_pressure;
block->e_to_p_pressure = baricuda_e_to_p_pressure;
#endif
// Compute direction bits for this block
uint8_t db = 0;
#if ENABLED(COREXY)
if (dx < 0) SBI(db, X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) SBI(db, Y_HEAD); // ...and Y
if (dz < 0) SBI(db, Z_AXIS);
if (dx + dy < 0) SBI(db, A_AXIS); // Motor A direction
if (dx - dy < 0) SBI(db, B_AXIS); // Motor B direction
#elif ENABLED(COREXZ)
if (dx < 0) SBI(db, X_HEAD); // Save the real Extruder (head) direction in X Axis
if (dy < 0) SBI(db, Y_AXIS);
if (dz < 0) SBI(db, Z_HEAD); // ...and Z
if (dx + dz < 0) SBI(db, A_AXIS); // Motor A direction
if (dx - dz < 0) SBI(db, C_AXIS); // Motor B direction
#else
if (dx < 0) SBI(db, X_AXIS);
if (dy < 0) SBI(db, Y_AXIS);
if (dz < 0) SBI(db, Z_AXIS);
#endif
if (de < 0) SBI(db, E_AXIS);
block->direction_bits = db;
block->active_extruder = extruder;
//enable active axes
#if ENABLED(COREXY)
if (block->steps[A_AXIS] || block->steps[B_AXIS]) {
enable_x();
enable_y();
}
#if DISABLED(Z_LATE_ENABLE)
if (block->steps[Z_AXIS]) enable_z();
#endif
#elif ENABLED(COREXZ)
if (block->steps[A_AXIS] || block->steps[C_AXIS]) {
enable_x();
enable_z();
}
if (block->steps[Y_AXIS]) enable_y();
#else
if (block->steps[X_AXIS]) enable_x();
if (block->steps[Y_AXIS]) enable_y();
#if DISABLED(Z_LATE_ENABLE)
if (block->steps[Z_AXIS]) enable_z();
#endif
#endif
// Enable extruder(s)
if (block->steps[E_AXIS]) {
#if ENABLED(DISABLE_INACTIVE_EXTRUDER) // Enable only the selected extruder
for (int i = 0; i < EXTRUDERS; i++)
if (g_uc_extruder_last_move[i] > 0) g_uc_extruder_last_move[i]--;
switch(extruder) {
case 0:
enable_e0();
#if ENABLED(DUAL_X_CARRIAGE)
if (extruder_duplication_enabled) {
enable_e1();
g_uc_extruder_last_move[1] = (BLOCK_BUFFER_SIZE) * 2;
}
#endif
g_uc_extruder_last_move[0] = (BLOCK_BUFFER_SIZE) * 2;
#if EXTRUDERS > 1
if (g_uc_extruder_last_move[1] == 0) disable_e1();
#if EXTRUDERS > 2
if (g_uc_extruder_last_move[2] == 0) disable_e2();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
#endif
#endif
break;
#if EXTRUDERS > 1
case 1:
enable_e1();
g_uc_extruder_last_move[1] = (BLOCK_BUFFER_SIZE) * 2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
#if EXTRUDERS > 2
if (g_uc_extruder_last_move[2] == 0) disable_e2();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
#endif
break;
#if EXTRUDERS > 2
case 2:
enable_e2();
g_uc_extruder_last_move[2] = (BLOCK_BUFFER_SIZE) * 2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
if (g_uc_extruder_last_move[1] == 0) disable_e1();
#if EXTRUDERS > 3
if (g_uc_extruder_last_move[3] == 0) disable_e3();
#endif
break;
#if EXTRUDERS > 3
case 3:
enable_e3();
g_uc_extruder_last_move[3] = (BLOCK_BUFFER_SIZE) * 2;
if (g_uc_extruder_last_move[0] == 0) disable_e0();
if (g_uc_extruder_last_move[1] == 0) disable_e1();
if (g_uc_extruder_last_move[2] == 0) disable_e2();
break;
#endif // EXTRUDERS > 3
#endif // EXTRUDERS > 2
#endif // EXTRUDERS > 1
}
#else
enable_e0();
enable_e1();
enable_e2();
enable_e3();
#endif
}
if (block->steps[E_AXIS])
NOLESS(feed_rate, min_feedrate);
else
NOLESS(feed_rate, min_travel_feedrate);
/**
* This part of the code calculates the total length of the movement.
* For cartesian bots, the X_AXIS is the real X movement and same for Y_AXIS.
* But for corexy bots, that is not true. The "X_AXIS" and "Y_AXIS" motors (that should be named to A_AXIS
* and B_AXIS) cannot be used for X and Y length, because A=X+Y and B=X-Y.
* So we need to create other 2 "AXIS", named X_HEAD and Y_HEAD, meaning the real displacement of the Head.
* Having the real displacement of the head, we can calculate the total movement length and apply the desired speed.
*/
#if ENABLED(COREXY)
float delta_mm[6];
delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
delta_mm[Y_HEAD] = dy / axis_steps_per_unit[B_AXIS];
delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
delta_mm[A_AXIS] = (dx + dy) / axis_steps_per_unit[A_AXIS];
delta_mm[B_AXIS] = (dx - dy) / axis_steps_per_unit[B_AXIS];
#elif ENABLED(COREXZ)
float delta_mm[6];
delta_mm[X_HEAD] = dx / axis_steps_per_unit[A_AXIS];
delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
delta_mm[Z_HEAD] = dz / axis_steps_per_unit[C_AXIS];
delta_mm[A_AXIS] = (dx + dz) / axis_steps_per_unit[A_AXIS];
delta_mm[C_AXIS] = (dx - dz) / axis_steps_per_unit[C_AXIS];
#else
float delta_mm[4];
delta_mm[X_AXIS] = dx / axis_steps_per_unit[X_AXIS];
delta_mm[Y_AXIS] = dy / axis_steps_per_unit[Y_AXIS];
delta_mm[Z_AXIS] = dz / axis_steps_per_unit[Z_AXIS];
#endif
delta_mm[E_AXIS] = (de / axis_steps_per_unit[E_AXIS]) * volumetric_multiplier[extruder] * extruder_multiplier[extruder] / 100.0;
if (block->steps[X_AXIS] <= dropsegments && block->steps[Y_AXIS] <= dropsegments && block->steps[Z_AXIS] <= dropsegments) {
block->millimeters = fabs(delta_mm[E_AXIS]);
}
else {
block->millimeters = sqrt(
#if ENABLED(COREXY)
square(delta_mm[X_HEAD]) + square(delta_mm[Y_HEAD]) + square(delta_mm[Z_AXIS])
#elif ENABLED(COREXZ)
square(delta_mm[X_HEAD]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_HEAD])
#else
square(delta_mm[X_AXIS]) + square(delta_mm[Y_AXIS]) + square(delta_mm[Z_AXIS])
#endif
);
}
float inverse_millimeters = 1.0 / block->millimeters; // Inverse millimeters to remove multiple divides
// Calculate moves/second for this move. No divide by zero due to previous checks.
float inverse_second = feed_rate * inverse_millimeters;
int moves_queued = movesplanned();
// Slow down when the buffer starts to empty, rather than wait at the corner for a buffer refill
#if ENABLED(OLD_SLOWDOWN) || ENABLED(SLOWDOWN)
bool mq = moves_queued > 1 && moves_queued < (BLOCK_BUFFER_SIZE) / 2;
#if ENABLED(OLD_SLOWDOWN)
if (mq) feed_rate *= 2.0 * moves_queued / (BLOCK_BUFFER_SIZE);
#endif
#if ENABLED(SLOWDOWN)
// segment time im micro seconds
unsigned long segment_time = lround(1000000.0/inverse_second);
if (mq) {
if (segment_time < min_segment_time) {
// buffer is draining, add extra time. The amount of time added increases if the buffer is still emptied more.
inverse_second = 1000000.0 / (segment_time + lround(2 * (min_segment_time - segment_time) / moves_queued));
#ifdef XY_FREQUENCY_LIMIT
segment_time = lround(1000000.0 / inverse_second);
#endif
}
}
#endif
#endif
block->nominal_speed = block->millimeters * inverse_second; // (mm/sec) Always > 0
block->nominal_rate = ceil(block->step_event_count * inverse_second); // (step/sec) Always > 0
#if ENABLED(FILAMENT_WIDTH_SENSOR)
static float filwidth_e_count = 0, filwidth_delay_dist = 0;
//FMM update ring buffer used for delay with filament measurements
if (extruder == FILAMENT_SENSOR_EXTRUDER_NUM && filwidth_delay_index2 >= 0) { //only for extruder with filament sensor and if ring buffer is initialized
const int MMD_CM = MAX_MEASUREMENT_DELAY + 1, MMD_MM = MMD_CM * 10;
// increment counters with next move in e axis
filwidth_e_count += delta_mm[E_AXIS];
filwidth_delay_dist += delta_mm[E_AXIS];
// Only get new measurements on forward E movement
if (filwidth_e_count > 0.0001) {
// Loop the delay distance counter (modulus by the mm length)
while (filwidth_delay_dist >= MMD_MM) filwidth_delay_dist -= MMD_MM;
// Convert into an index into the measurement array
filwidth_delay_index1 = (int)(filwidth_delay_dist / 10.0 + 0.0001);
// If the index has changed (must have gone forward)...
if (filwidth_delay_index1 != filwidth_delay_index2) {
filwidth_e_count = 0; // Reset the E movement counter
int8_t meas_sample = thermalManager.widthFil_to_size_ratio() - 100; // Subtract 100 to reduce magnitude - to store in a signed char
do {
filwidth_delay_index2 = (filwidth_delay_index2 + 1) % MMD_CM; // The next unused slot
measurement_delay[filwidth_delay_index2] = meas_sample; // Store the measurement
} while (filwidth_delay_index1 != filwidth_delay_index2); // More slots to fill?
}
}
}
#endif
// Calculate and limit speed in mm/sec for each axis
float current_speed[NUM_AXIS];
float speed_factor = 1.0; //factor <=1 do decrease speed
for (int i = 0; i < NUM_AXIS; i++) {
current_speed[i] = delta_mm[i] * inverse_second;
float cs = fabs(current_speed[i]), mf = max_feedrate[i];
if (cs > mf) speed_factor = min(speed_factor, mf / cs);
}
// Max segement time in us.
#ifdef XY_FREQUENCY_LIMIT
// Check and limit the xy direction change frequency
unsigned char direction_change = block->direction_bits ^ old_direction_bits;
old_direction_bits = block->direction_bits;
segment_time = lround((float)segment_time / speed_factor);
long xs0 = axis_segment_time[X_AXIS][0],
xs1 = axis_segment_time[X_AXIS][1],
xs2 = axis_segment_time[X_AXIS][2],
ys0 = axis_segment_time[Y_AXIS][0],
ys1 = axis_segment_time[Y_AXIS][1],
ys2 = axis_segment_time[Y_AXIS][2];
if (TEST(direction_change, X_AXIS)) {
xs2 = axis_segment_time[X_AXIS][2] = xs1;
xs1 = axis_segment_time[X_AXIS][1] = xs0;
xs0 = 0;
}
xs0 = axis_segment_time[X_AXIS][0] = xs0 + segment_time;
if (TEST(direction_change, Y_AXIS)) {
ys2 = axis_segment_time[Y_AXIS][2] = axis_segment_time[Y_AXIS][1];
ys1 = axis_segment_time[Y_AXIS][1] = axis_segment_time[Y_AXIS][0];
ys0 = 0;
}
ys0 = axis_segment_time[Y_AXIS][0] = ys0 + segment_time;
long max_x_segment_time = max(xs0, max(xs1, xs2)),
max_y_segment_time = max(ys0, max(ys1, ys2)),
min_xy_segment_time = min(max_x_segment_time, max_y_segment_time);
if (min_xy_segment_time < MAX_FREQ_TIME) {
float low_sf = speed_factor * min_xy_segment_time / (MAX_FREQ_TIME);
speed_factor = min(speed_factor, low_sf);
}
#endif // XY_FREQUENCY_LIMIT
// Correct the speed
if (speed_factor < 1.0) {
for (unsigned char i = 0; i < NUM_AXIS; i++) current_speed[i] *= speed_factor;
block->nominal_speed *= speed_factor;
block->nominal_rate *= speed_factor;
}
// Compute and limit the acceleration rate for the trapezoid generator.
float steps_per_mm = block->step_event_count / block->millimeters;
long bsx = block->steps[X_AXIS], bsy = block->steps[Y_AXIS], bsz = block->steps[Z_AXIS], bse = block->steps[E_AXIS];
if (bsx == 0 && bsy == 0 && bsz == 0) {
block->acceleration_st = ceil(retract_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else if (bse == 0) {
block->acceleration_st = ceil(travel_acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
else {
block->acceleration_st = ceil(acceleration * steps_per_mm); // convert to: acceleration steps/sec^2
}
// Limit acceleration per axis
unsigned long acc_st = block->acceleration_st,
xsteps = axis_steps_per_sqr_second[X_AXIS],
ysteps = axis_steps_per_sqr_second[Y_AXIS],
zsteps = axis_steps_per_sqr_second[Z_AXIS],
esteps = axis_steps_per_sqr_second[E_AXIS],
allsteps = block->step_event_count;
if (xsteps < (acc_st * bsx) / allsteps) acc_st = (xsteps * allsteps) / bsx;
if (ysteps < (acc_st * bsy) / allsteps) acc_st = (ysteps * allsteps) / bsy;
if (zsteps < (acc_st * bsz) / allsteps) acc_st = (zsteps * allsteps) / bsz;
if (esteps < (acc_st * bse) / allsteps) acc_st = (esteps * allsteps) / bse;
block->acceleration_st = acc_st;
block->acceleration = acc_st / steps_per_mm;
block->acceleration_rate = (long)(acc_st * 16777216.0 / (F_CPU / 8.0));
#if 0 // Use old jerk for now
float junction_deviation = 0.1;
// Compute path unit vector
double unit_vec[3];
unit_vec[X_AXIS] = delta_mm[X_AXIS] * inverse_millimeters;
unit_vec[Y_AXIS] = delta_mm[Y_AXIS] * inverse_millimeters;
unit_vec[Z_AXIS] = delta_mm[Z_AXIS] * inverse_millimeters;
// Compute maximum allowable entry speed at junction by centripetal acceleration approximation.
// Let a circle be tangent to both previous and current path line segments, where the junction
// deviation is defined as the distance from the junction to the closest edge of the circle,
// collinear with the circle center. The circular segment joining the two paths represents the
// path of centripetal acceleration. Solve for max velocity based on max acceleration about the
// radius of the circle, defined indirectly by junction deviation. This may be also viewed as
// path width or max_jerk in the previous grbl version. This approach does not actually deviate
// from path, but used as a robust way to compute cornering speeds, as it takes into account the
// nonlinearities of both the junction angle and junction velocity.
double vmax_junction = MINIMUM_PLANNER_SPEED; // Set default max junction speed
// Skip first block or when previous_nominal_speed is used as a flag for homing and offset cycles.
if ((block_buffer_head != block_buffer_tail) && (previous_nominal_speed > 0.0)) {
// Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
// NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
double cos_theta = - previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
- previous_unit_vec[Y_AXIS] * unit_vec[Y_AXIS]
- previous_unit_vec[Z_AXIS] * unit_vec[Z_AXIS] ;
// Skip and use default max junction speed for 0 degree acute junction.
if (cos_theta < 0.95) {
vmax_junction = min(previous_nominal_speed, block->nominal_speed);
// Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
if (cos_theta > -0.95) {
// Compute maximum junction velocity based on maximum acceleration and junction deviation
double sin_theta_d2 = sqrt(0.5 * (1.0 - cos_theta)); // Trig half angle identity. Always positive.
vmax_junction = min(vmax_junction,
sqrt(block->acceleration * junction_deviation * sin_theta_d2 / (1.0 - sin_theta_d2)));
}
}
}
#endif
// Start with a safe speed
float vmax_junction = max_xy_jerk / 2;
float vmax_junction_factor = 1.0;
float mz2 = max_z_jerk / 2, me2 = max_e_jerk / 2;
float csz = current_speed[Z_AXIS], cse = current_speed[E_AXIS];
if (fabs(csz) > mz2) vmax_junction = min(vmax_junction, mz2);
if (fabs(cse) > me2) vmax_junction = min(vmax_junction, me2);
vmax_junction = min(vmax_junction, block->nominal_speed);
float safe_speed = vmax_junction;
if ((moves_queued > 1) && (previous_nominal_speed > 0.0001)) {
float dsx = current_speed[X_AXIS] - previous_speed[X_AXIS],
dsy = current_speed[Y_AXIS] - previous_speed[Y_AXIS],
dsz = fabs(csz - previous_speed[Z_AXIS]),
dse = fabs(cse - previous_speed[E_AXIS]),
jerk = sqrt(dsx * dsx + dsy * dsy);
// if ((fabs(previous_speed[X_AXIS]) > 0.0001) || (fabs(previous_speed[Y_AXIS]) > 0.0001)) {
vmax_junction = block->nominal_speed;
// }
if (jerk > max_xy_jerk) vmax_junction_factor = max_xy_jerk / jerk;
if (dsz > max_z_jerk) vmax_junction_factor = min(vmax_junction_factor, max_z_jerk / dsz);
if (dse > max_e_jerk) vmax_junction_factor = min(vmax_junction_factor, max_e_jerk / dse);
vmax_junction = min(previous_nominal_speed, vmax_junction * vmax_junction_factor); // Limit speed to max previous speed
}
block->max_entry_speed = vmax_junction;
// Initialize block entry speed. Compute based on deceleration to user-defined MINIMUM_PLANNER_SPEED.
double v_allowable = max_allowable_speed(-block->acceleration, MINIMUM_PLANNER_SPEED, block->millimeters);
block->entry_speed = min(vmax_junction, v_allowable);
// Initialize planner efficiency flags
// Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
// If a block can de/ac-celerate from nominal speed to zero within the length of the block, then
// the current block and next block junction speeds are guaranteed to always be at their maximum
// junction speeds in deceleration and acceleration, respectively. This is due to how the current
// block nominal speed limits both the current and next maximum junction speeds. Hence, in both
// the reverse and forward planners, the corresponding block junction speed will always be at the
// the maximum junction speed and may always be ignored for any speed reduction checks.
block->nominal_length_flag = (block->nominal_speed <= v_allowable);
block->recalculate_flag = true; // Always calculate trapezoid for new block
// Update previous path unit_vector and nominal speed
for (int i = 0; i < NUM_AXIS; i++) previous_speed[i] = current_speed[i];
previous_nominal_speed = block->nominal_speed;
#if ENABLED(ADVANCE)
// Calculate advance rate
if (!bse || (!bsx && !bsy && !bsz)) {
block->advance_rate = 0;
block->advance = 0;
}
else {
long acc_dist = estimate_acceleration_distance(0, block->nominal_rate, block->acceleration_st);
float advance = ((STEPS_PER_CUBIC_MM_E) * (EXTRUDER_ADVANCE_K)) * (cse * cse * (EXTRUSION_AREA) * (EXTRUSION_AREA)) * 256;
block->advance = advance;
block->advance_rate = acc_dist ? advance / (float)acc_dist : 0;
}
/**
SERIAL_ECHO_START;
SERIAL_ECHOPGM("advance :");
SERIAL_ECHO(block->advance/256.0);
SERIAL_ECHOPGM("advance rate :");
SERIAL_ECHOLN(block->advance_rate/256.0);
*/
#endif // ADVANCE
calculate_trapezoid_for_block(block, block->entry_speed / block->nominal_speed, safe_speed / block->nominal_speed);
// Move buffer head
block_buffer_head = next_buffer_head;
// Update position
for (int i = 0; i < NUM_AXIS; i++) position[i] = target[i];
recalculate();
stepper.wake_up();
} // buffer_line()
int
pfkey_msg_parse(struct sadb_msg *pfkey_msg,
struct pf_key_ext_parsers_def *ext_parsers[],
struct sadb_ext *extensions[],
int dir)
{
int error = 0;
int remain;
struct sadb_ext *pfkey_ext;
int extensions_seen = 0;
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_msg_parse: "
"parsing message ver=%d, type=%d(%s), errno=%d, satype=%d(%s), len=%d, res=%d, seq=%d, pid=%d.\n",
pfkey_msg->sadb_msg_version,
pfkey_msg->sadb_msg_type,
pfkey_v2_sadb_type_string(pfkey_msg->sadb_msg_type),
pfkey_msg->sadb_msg_errno,
pfkey_msg->sadb_msg_satype,
satype2name(pfkey_msg->sadb_msg_satype),
pfkey_msg->sadb_msg_len,
pfkey_msg->sadb_msg_reserved,
pfkey_msg->sadb_msg_seq,
pfkey_msg->sadb_msg_pid);
if(ext_parsers == NULL) ext_parsers = ext_default_parsers;
pfkey_extensions_init(extensions);
remain = pfkey_msg->sadb_msg_len;
remain -= sizeof(struct sadb_msg) / IPSEC_PFKEYv2_ALIGN;
pfkey_ext = (struct sadb_ext*)((char*)pfkey_msg +
sizeof(struct sadb_msg));
extensions[0] = (struct sadb_ext *) pfkey_msg;
if(pfkey_msg->sadb_msg_version != PF_KEY_V2) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"not PF_KEY_V2 msg, found %d, should be %d.\n",
pfkey_msg->sadb_msg_version,
PF_KEY_V2);
SENDERR(EINVAL);
}
if(!pfkey_msg->sadb_msg_type) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"msg type not set, must be non-zero..\n");
SENDERR(EINVAL);
}
if(pfkey_msg->sadb_msg_type > SADB_MAX) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"msg type=%d > max=%d.\n",
pfkey_msg->sadb_msg_type,
SADB_MAX);
SENDERR(EINVAL);
}
switch(pfkey_msg->sadb_msg_type) {
case SADB_GETSPI:
case SADB_UPDATE:
case SADB_ADD:
case SADB_DELETE:
case SADB_GET:
case SADB_X_GRPSA:
case SADB_X_ADDFLOW:
if(!satype2proto(pfkey_msg->sadb_msg_satype)) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"satype %d conversion to proto failed for msg_type %d (%s).\n",
pfkey_msg->sadb_msg_satype,
pfkey_msg->sadb_msg_type,
pfkey_v2_sadb_type_string(pfkey_msg->sadb_msg_type));
SENDERR(EINVAL);
} else {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"satype %d(%s) conversion to proto gives %d for msg_type %d(%s).\n",
pfkey_msg->sadb_msg_satype,
satype2name(pfkey_msg->sadb_msg_satype),
satype2proto(pfkey_msg->sadb_msg_satype),
pfkey_msg->sadb_msg_type,
pfkey_v2_sadb_type_string(pfkey_msg->sadb_msg_type));
}
case SADB_ACQUIRE:
case SADB_REGISTER:
case SADB_EXPIRE:
if(!pfkey_msg->sadb_msg_satype) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"satype is zero, must be non-zero for msg_type %d(%s).\n",
pfkey_msg->sadb_msg_type,
pfkey_v2_sadb_type_string(pfkey_msg->sadb_msg_type));
SENDERR(EINVAL);
}
default:
}
/* errno must not be set in downward messages */
/* this is not entirely true... a response to an ACQUIRE could return an error */
if((dir == EXT_BITS_IN) && (pfkey_msg->sadb_msg_type != SADB_ACQUIRE) && pfkey_msg->sadb_msg_errno) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"errno set to %d.\n",
pfkey_msg->sadb_msg_errno);
SENDERR(EINVAL);
}
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_msg_parse: "
"remain=%d, ext_type=%d(%s), ext_len=%d.\n",
remain,
pfkey_ext->sadb_ext_type,
pfkey_v2_sadb_ext_string(pfkey_ext->sadb_ext_type),
pfkey_ext->sadb_ext_len);
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_msg_parse: "
"extensions permitted=%08x, required=%08x.\n",
extensions_bitmaps[dir][EXT_BITS_PERM][pfkey_msg->sadb_msg_type],
extensions_bitmaps[dir][EXT_BITS_REQ][pfkey_msg->sadb_msg_type]);
extensions_seen = 1;
while( (remain * IPSEC_PFKEYv2_ALIGN) >= sizeof(struct sadb_ext) ) {
/* Is there enough message left to support another extension header? */
if(remain < pfkey_ext->sadb_ext_len) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"remain %d less than ext len %d.\n",
remain, pfkey_ext->sadb_ext_len);
SENDERR(EINVAL);
}
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_msg_parse: "
"parsing ext type=%d remain=%d.\n",
pfkey_ext->sadb_ext_type,
remain);
/* Is the extension header type valid? */
if((pfkey_ext->sadb_ext_type > SADB_EXT_MAX) || (!pfkey_ext->sadb_ext_type)) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"ext type %d invalid, SADB_EXT_MAX=%d.\n",
pfkey_ext->sadb_ext_type, SADB_EXT_MAX);
SENDERR(EINVAL);
}
/* Have we already seen this type of extension? */
if((extensions_seen & ( 1 << pfkey_ext->sadb_ext_type )) != 0)
{
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"ext type %d already seen.\n",
pfkey_ext->sadb_ext_type);
SENDERR(EINVAL);
}
/* Do I even know about this type of extension? */
if(ext_parsers[pfkey_ext->sadb_ext_type]==NULL) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"ext type %d unknown, ignoring.\n",
pfkey_ext->sadb_ext_type);
goto next_ext;
}
/* Is this type of extension permitted for this type of message? */
if(!(extensions_bitmaps[dir][EXT_BITS_PERM][pfkey_msg->sadb_msg_type] &
1<<pfkey_ext->sadb_ext_type)) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"ext type %d not permitted, exts_perm_in=%08x, 1<<type=%08x\n",
pfkey_ext->sadb_ext_type,
extensions_bitmaps[dir][EXT_BITS_PERM][pfkey_msg->sadb_msg_type],
1<<pfkey_ext->sadb_ext_type);
SENDERR(EINVAL);
}
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_msg_parse: "
"About to parse extension %d %p with parser %s.\n",
pfkey_ext->sadb_ext_type,
pfkey_ext,
ext_parsers[pfkey_ext->sadb_ext_type]->parser_name);
/* Parse the extension */
if((error =
(*ext_parsers[pfkey_ext->sadb_ext_type]->parser)(pfkey_ext))) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"extension parsing for type %d failed with error %d.\n",
pfkey_ext->sadb_ext_type, error);
SENDERR(-error);
}
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_msg_parse: "
"Extension %d parsed.\n",
pfkey_ext->sadb_ext_type);
/* Mark that we have seen this extension and remember the header location */
extensions_seen |= ( 1 << pfkey_ext->sadb_ext_type );
extensions[pfkey_ext->sadb_ext_type] = pfkey_ext;
next_ext:
/* Calculate how much message remains */
remain -= pfkey_ext->sadb_ext_len;
if(!remain) {
break;
}
/* Find the next extension header */
pfkey_ext = (struct sadb_ext*)((char*)pfkey_ext +
pfkey_ext->sadb_ext_len * IPSEC_PFKEYv2_ALIGN);
}
if(remain) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"unexpected remainder of %d.\n",
remain);
/* why is there still something remaining? */
SENDERR(EINVAL);
}
/* check required extensions */
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_msg_parse: "
"extensions permitted=%08x, seen=%08x, required=%08x.\n",
extensions_bitmaps[dir][EXT_BITS_PERM][pfkey_msg->sadb_msg_type],
extensions_seen,
extensions_bitmaps[dir][EXT_BITS_REQ][pfkey_msg->sadb_msg_type]);
/* don't check further if it is an error return message since it
may not have a body */
if(pfkey_msg->sadb_msg_errno) {
SENDERR(-error);
}
if((extensions_seen &
extensions_bitmaps[dir][EXT_BITS_REQ][pfkey_msg->sadb_msg_type]) !=
extensions_bitmaps[dir][EXT_BITS_REQ][pfkey_msg->sadb_msg_type]) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"required extensions missing:%08x.\n",
extensions_bitmaps[dir][EXT_BITS_REQ][pfkey_msg->sadb_msg_type] -
(extensions_seen &
extensions_bitmaps[dir][EXT_BITS_REQ][pfkey_msg->sadb_msg_type]));
SENDERR(EINVAL);
}
if((dir == EXT_BITS_IN) && (pfkey_msg->sadb_msg_type == SADB_X_DELFLOW)
&& ((extensions_seen & SADB_X_EXT_ADDRESS_DELFLOW)
!= SADB_X_EXT_ADDRESS_DELFLOW)
&& (((extensions_seen & (1<<SADB_EXT_SA)) != (1<<SADB_EXT_SA))
|| ((((struct sadb_sa*)extensions[SADB_EXT_SA])->sadb_sa_flags
& SADB_X_SAFLAGS_CLEARFLOW)
!= SADB_X_SAFLAGS_CLEARFLOW))) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"required SADB_X_DELFLOW extensions missing: either %08x must be present or %08x must be present with SADB_X_SAFLAGS_CLEARFLOW set.\n",
SADB_X_EXT_ADDRESS_DELFLOW
- (extensions_seen & SADB_X_EXT_ADDRESS_DELFLOW),
(1<<SADB_EXT_SA) - (extensions_seen & (1<<SADB_EXT_SA)));
SENDERR(EINVAL);
}
switch(pfkey_msg->sadb_msg_type) {
case SADB_ADD:
case SADB_UPDATE:
/* check maturity */
if(((struct sadb_sa*)extensions[SADB_EXT_SA])->sadb_sa_state !=
SADB_SASTATE_MATURE) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"state=%d for add or update should be MATURE=%d.\n",
((struct sadb_sa*)extensions[SADB_EXT_SA])->sadb_sa_state,
SADB_SASTATE_MATURE);
SENDERR(EINVAL);
}
/* check AH and ESP */
switch(((struct sadb_msg*)extensions[SADB_EXT_RESERVED])->sadb_msg_satype) {
case SADB_SATYPE_AH:
if(!(((struct sadb_sa*)extensions[SADB_EXT_SA]) &&
((struct sadb_sa*)extensions[SADB_EXT_SA])->sadb_sa_auth !=
SADB_AALG_NONE)) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"auth alg is zero, must be non-zero for AH SAs.\n");
SENDERR(EINVAL);
}
if(((struct sadb_sa*)(extensions[SADB_EXT_SA]))->sadb_sa_encrypt !=
SADB_EALG_NONE) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"AH handed encalg=%d, must be zero.\n",
((struct sadb_sa*)(extensions[SADB_EXT_SA]))->sadb_sa_encrypt);
SENDERR(EINVAL);
}
break;
case SADB_SATYPE_ESP:
if(!(((struct sadb_sa*)extensions[SADB_EXT_SA]) &&
((struct sadb_sa*)extensions[SADB_EXT_SA])->sadb_sa_encrypt !=
SADB_EALG_NONE)) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"encrypt alg=%d is zero, must be non-zero for ESP=%d SAs.\n",
((struct sadb_sa*)extensions[SADB_EXT_SA])->sadb_sa_encrypt,
((struct sadb_msg*)extensions[SADB_EXT_RESERVED])->sadb_msg_satype);
SENDERR(EINVAL);
}
if((((struct sadb_sa*)(extensions[SADB_EXT_SA]))->sadb_sa_encrypt ==
SADB_EALG_NULL) &&
(((struct sadb_sa*)(extensions[SADB_EXT_SA]))->sadb_sa_auth ==
SADB_AALG_NONE) ) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"ESP handed encNULL+authNONE, illegal combination.\n");
SENDERR(EINVAL);
}
break;
case SADB_X_SATYPE_COMP:
if(!(((struct sadb_sa*)extensions[SADB_EXT_SA]) &&
((struct sadb_sa*)extensions[SADB_EXT_SA])->sadb_sa_encrypt !=
SADB_EALG_NONE)) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"encrypt alg=%d is zero, must be non-zero for COMP=%d SAs.\n",
((struct sadb_sa*)extensions[SADB_EXT_SA])->sadb_sa_encrypt,
((struct sadb_msg*)extensions[SADB_EXT_RESERVED])->sadb_msg_satype);
SENDERR(EINVAL);
}
if(((struct sadb_sa*)(extensions[SADB_EXT_SA]))->sadb_sa_auth !=
SADB_AALG_NONE) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"COMP handed auth=%d, must be zero.\n",
((struct sadb_sa*)(extensions[SADB_EXT_SA]))->sadb_sa_auth);
SENDERR(EINVAL);
}
break;
default:
}
if(ntohl(((struct sadb_sa*)(extensions[SADB_EXT_SA]))->sadb_sa_spi) <= 255) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_msg_parse: "
"spi=%08lx must be > 255.\n",
ntohl(((struct sadb_sa*)(extensions[SADB_EXT_SA]))->sadb_sa_spi));
SENDERR(EINVAL);
}
default:
}
errlab:
return error;
}
DEBUG_NO_STATIC int
pfkey_sa_parse(struct sadb_ext *pfkey_ext)
{
int error = 0;
struct k_sadb_sa *pfkey_sa = (struct k_sadb_sa *)pfkey_ext;
/* sanity checks... */
if(!pfkey_sa) {
ERROR("pfkey_sa_parse: "
"NULL pointer passed in.\n");
SENDERR(EINVAL);
}
if(pfkey_sa->sadb_sa_len !=sizeof(struct k_sadb_sa)/IPSEC_PFKEYv2_ALIGN
&& pfkey_sa->sadb_sa_len!=sizeof(struct sadb_sa)/IPSEC_PFKEYv2_ALIGN) {
ERROR(
"pfkey_sa_parse: "
"length wrong pfkey_sa->sadb_sa_len=%d sizeof(struct sadb_sa)=%d.\n",
pfkey_sa->sadb_sa_len,
(int)sizeof(struct k_sadb_sa));
SENDERR(EINVAL);
}
#if K_SADB_EALG_MAX < 255
if(pfkey_sa->sadb_sa_encrypt > K_SADB_EALG_MAX) {
ERROR(
"pfkey_sa_parse: "
"pfkey_sa->sadb_sa_encrypt=%d > K_SADB_EALG_MAX=%d.\n",
pfkey_sa->sadb_sa_encrypt,
K_SADB_EALG_MAX);
SENDERR(EINVAL);
}
#endif
#if K_SADB_AALG_MAX < 255
if(pfkey_sa->sadb_sa_auth > K_SADB_AALG_MAX) {
ERROR(
"pfkey_sa_parse: "
"pfkey_sa->sadb_sa_auth=%d > K_SADB_AALG_MAX=%d.\n",
pfkey_sa->sadb_sa_auth,
K_SADB_AALG_MAX);
SENDERR(EINVAL);
}
#endif
#if K_SADB_SASTATE_MAX < 255
if(pfkey_sa->sadb_sa_state > K_SADB_SASTATE_MAX) {
ERROR(
"pfkey_sa_parse: "
"state=%d exceeds MAX=%d.\n",
pfkey_sa->sadb_sa_state,
K_SADB_SASTATE_MAX);
SENDERR(EINVAL);
}
#endif
if(pfkey_sa->sadb_sa_state == K_SADB_SASTATE_DEAD) {
ERROR(
"pfkey_sa_parse: "
"state=%d is DEAD=%d.\n",
pfkey_sa->sadb_sa_state,
K_SADB_SASTATE_DEAD);
SENDERR(EINVAL);
}
if(pfkey_sa->sadb_sa_replay > 64) {
ERROR(
"pfkey_sa_parse: "
"replay window size: %d -- must be 0 <= size <= 64\n",
pfkey_sa->sadb_sa_replay);
SENDERR(EINVAL);
}
if(! ((pfkey_sa->sadb_sa_exttype == K_SADB_EXT_SA) ||
(pfkey_sa->sadb_sa_exttype == K_SADB_X_EXT_SA2)))
{
ERROR(
"pfkey_sa_parse: "
"unknown exttype=%d, expecting K_SADB_EXT_SA=%d or K_SADB_X_EXT_SA2=%d.\n",
pfkey_sa->sadb_sa_exttype,
K_SADB_EXT_SA,
K_SADB_X_EXT_SA2);
SENDERR(EINVAL);
}
if(pfkey_sa->sadb_sa_len > sizeof(struct sadb_sa)/IPSEC_PFKEYv2_ALIGN) {
if(pfkey_sa->sadb_x_sa_ref == IPSEC_SAREF_NULL ||
pfkey_sa->sadb_x_sa_ref == ~(IPSEC_SAREF_NULL))
{
pfkey_sa->sadb_x_sa_ref = IPSEC_SAREF_NULL;
}
}
if((IPSEC_SAREF_NULL != pfkey_sa->sadb_x_sa_ref)
&& (pfkey_sa->sadb_x_sa_ref >= (1 << IPSEC_SA_REF_TABLE_IDX_WIDTH)))
{
ERROR(
"pfkey_sa_parse: "
"SAref=%d must be (SAref == IPSEC_SAREF_NULL(%d) || SAref < IPSEC_SA_REF_TABLE_NUM_ENTRIES(%d)).\n",
pfkey_sa->sadb_x_sa_ref,
IPSEC_SAREF_NULL,
IPSEC_SA_REF_TABLE_NUM_ENTRIES);
SENDERR(EINVAL);
}
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_sa_parse: "
"successfully found len=%d exttype=%d(%s) spi=%08lx replay=%d state=%d auth=%d encrypt=%d flags=%d ref=%d.\n",
pfkey_sa->sadb_sa_len,
pfkey_sa->sadb_sa_exttype,
pfkey_v2_sadb_ext_string(pfkey_sa->sadb_sa_exttype),
(long unsigned int)ntohl(pfkey_sa->sadb_sa_spi),
pfkey_sa->sadb_sa_replay,
pfkey_sa->sadb_sa_state,
pfkey_sa->sadb_sa_auth,
pfkey_sa->sadb_sa_encrypt,
pfkey_sa->sadb_sa_flags,
pfkey_sa->sadb_x_sa_ref);
errlab:
return error;
}
DEBUG_NO_STATIC int
pfkey_x_satype_parse(struct sadb_ext *pfkey_ext)
{
int error = 0;
int i;
struct sadb_x_satype *pfkey_x_satype = (struct sadb_x_satype *)pfkey_ext;
DEBUGGING(PF_KEY_DEBUG_PARSE_FLOW,
"pfkey_x_satype_parse: enter\n");
/* sanity checks... */
if(pfkey_x_satype->sadb_x_satype_len !=
sizeof(struct sadb_x_satype) / IPSEC_PFKEYv2_ALIGN) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_x_satype_parse: "
"size wrong ext_len=%d, key_ext_len=%d.\n",
pfkey_x_satype->sadb_x_satype_len,
(int)sizeof(struct sadb_x_satype));
SENDERR(EINVAL);
}
if(!pfkey_x_satype->sadb_x_satype_satype) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_x_satype_parse: "
"satype is zero, must be non-zero.\n");
SENDERR(EINVAL);
}
if(pfkey_x_satype->sadb_x_satype_satype > K_SADB_SATYPE_MAX) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_x_satype_parse: "
"satype %d > max %d, invalid.\n",
pfkey_x_satype->sadb_x_satype_satype, K_SADB_SATYPE_MAX);
SENDERR(EINVAL);
}
if(!(satype2proto(pfkey_x_satype->sadb_x_satype_satype))) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_x_satype_parse: "
"proto lookup from satype=%d failed.\n",
pfkey_x_satype->sadb_x_satype_satype);
SENDERR(EINVAL);
}
for(i = 0; i < 3; i++) {
if(pfkey_x_satype->sadb_x_satype_reserved[i]) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_x_satype_parse: "
"reserved[%d]=%d must be set to zero.\n",
i, pfkey_x_satype->sadb_x_satype_reserved[i]);
SENDERR(EINVAL);
}
}
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_x_satype_parse: "
"len=%u ext=%u(%s) satype=%u(%s) res=%u,%u,%u.\n",
pfkey_x_satype->sadb_x_satype_len,
pfkey_x_satype->sadb_x_satype_exttype,
pfkey_v2_sadb_ext_string(pfkey_x_satype->sadb_x_satype_exttype),
pfkey_x_satype->sadb_x_satype_satype,
satype2name(pfkey_x_satype->sadb_x_satype_satype),
pfkey_x_satype->sadb_x_satype_reserved[0],
pfkey_x_satype->sadb_x_satype_reserved[1],
pfkey_x_satype->sadb_x_satype_reserved[2]);
errlab:
return error;
}
/**
* M501 - Retrieve Configuration
*/
void Config_RetrieveSettings() {
int i = EEPROM_OFFSET;
char stored_ver[6];
uint16_t stored_checksum;
EEPROM_READ_VAR(i, stored_ver);
EEPROM_READ_VAR(i, stored_checksum);
if (DEBUGGING(INFO)) {
ECHO_SMV(INFO, "Version: [", version);
ECHO_MV("] Stored version: [", stored_ver);
ECHO_EM("]");
}
if (strncmp(version, stored_ver, 5) != 0) {
Config_ResetDefault();
}
else {
float dummy = 0;
eeprom_checksum = 0; // clear before reading first "real data"
// version number match
EEPROM_READ_VAR(i, planner.axis_steps_per_mm);
EEPROM_READ_VAR(i, planner.max_feedrate);
EEPROM_READ_VAR(i, planner.max_acceleration_mm_per_s2);
EEPROM_READ_VAR(i, planner.acceleration);
EEPROM_READ_VAR(i, planner.retract_acceleration);
EEPROM_READ_VAR(i, planner.travel_acceleration);
EEPROM_READ_VAR(i, planner.min_feedrate);
EEPROM_READ_VAR(i, planner.min_travel_feedrate);
EEPROM_READ_VAR(i, planner.min_segment_time);
EEPROM_READ_VAR(i, planner.max_xy_jerk);
EEPROM_READ_VAR(i, planner.max_z_jerk);
EEPROM_READ_VAR(i, planner.max_e_jerk);
EEPROM_READ_VAR(i, home_offset);
EEPROM_READ_VAR(i, hotend_offset);
#if ENABLED(MESH_BED_LEVELING)
uint8_t mesh_num_x = 0, mesh_num_y = 0;
EEPROM_READ_VAR(i, mbl.status);
EEPROM_READ_VAR(i, mbl.z_offset);
EEPROM_READ_VAR(i, mesh_num_x);
EEPROM_READ_VAR(i, mesh_num_y);
EEPROM_READ_VAR(i, mbl.z_values);
#endif
#if HEATER_USES_AD595
EEPROM_READ_VAR(i, ad595_offset);
EEPROM_READ_VAR(i, ad595_gain);
for (int8_t h = 0; h < HOTENDS; h++)
if (ad595_gain[h] == 0) ad595_gain[h] == TEMP_SENSOR_AD595_GAIN;
#endif
#if MECH(DELTA)
EEPROM_READ_VAR(i, endstop_adj);
EEPROM_READ_VAR(i, delta_radius);
EEPROM_READ_VAR(i, delta_diagonal_rod);
EEPROM_READ_VAR(i, sw_endstop_max);
EEPROM_READ_VAR(i, tower_adj);
EEPROM_READ_VAR(i, diagrod_adj);
#endif //DELTA
#if HASNT(BED_PROBE)
float zprobe_zoffset = 0;
#endif
EEPROM_READ_VAR(i, zprobe_zoffset);
#if DISABLED(ULTIPANEL)
int plaPreheatHotendTemp, plaPreheatHPBTemp, plaPreheatFanSpeed,
absPreheatHotendTemp, absPreheatHPBTemp, absPreheatFanSpeed,
gumPreheatHotendTemp, gumPreheatHPBTemp, gumPreheatFanSpeed;
#endif
EEPROM_READ_VAR(i, plaPreheatHotendTemp);
EEPROM_READ_VAR(i, plaPreheatHPBTemp);
EEPROM_READ_VAR(i, plaPreheatFanSpeed);
EEPROM_READ_VAR(i, absPreheatHotendTemp);
EEPROM_READ_VAR(i, absPreheatHPBTemp);
EEPROM_READ_VAR(i, absPreheatFanSpeed);
EEPROM_READ_VAR(i, gumPreheatHotendTemp);
EEPROM_READ_VAR(i, gumPreheatHPBTemp);
EEPROM_READ_VAR(i, gumPreheatFanSpeed);
#if ENABLED(PIDTEMP)
for (int8_t h = 0; h < HOTENDS; h++) {
EEPROM_READ_VAR(i, PID_PARAM(Kp, h));
EEPROM_READ_VAR(i, PID_PARAM(Ki, h));
EEPROM_READ_VAR(i, PID_PARAM(Kd, h));
EEPROM_READ_VAR(i, PID_PARAM(Kc, h));
}
#endif // PIDTEMP
#if DISABLED(PID_ADD_EXTRUSION_RATE)
int lpq_len;
#endif
EEPROM_READ_VAR(i, lpq_len);
#if ENABLED(PIDTEMPBED)
EEPROM_READ_VAR(i, bedKp);
EEPROM_READ_VAR(i, bedKi);
EEPROM_READ_VAR(i, bedKd);
#endif
#if ENABLED(PIDTEMPCHAMBER)
EEPROM_READ_VAR(i, chamberKp);
EEPROM_READ_VAR(i, chamberKi);
EEPROM_READ_VAR(i, chamberKd);
#endif
#if ENABLED(PIDTEMPCOOLER)
EEPROM_READ_VAR(i, coolerKp);
EEPROM_READ_VAR(i, coolerKi);
EEPROM_READ_VAR(i, coolerKd);
#endif
#if HASNT(LCD_CONTRAST)
int lcd_contrast;
#endif
EEPROM_READ_VAR(i, lcd_contrast);
#if MECH(SCARA)
EEPROM_READ_VAR(i, axis_scaling); // 3 floats
#endif
#if ENABLED(FWRETRACT)
EEPROM_READ_VAR(i, autoretract_enabled);
EEPROM_READ_VAR(i, retract_length);
#if EXTRUDERS > 1
EEPROM_READ_VAR(i, retract_length_swap);
#else
EEPROM_READ_VAR(i, dummy);
#endif
EEPROM_READ_VAR(i, retract_feedrate);
EEPROM_READ_VAR(i, retract_zlift);
EEPROM_READ_VAR(i, retract_recover_length);
#if EXTRUDERS > 1
EEPROM_READ_VAR(i, retract_recover_length_swap);
#else
EEPROM_READ_VAR(i, dummy);
#endif
EEPROM_READ_VAR(i, retract_recover_feedrate);
#endif // FWRETRACT
EEPROM_READ_VAR(i, volumetric_enabled);
for (int8_t e = 0; e < EXTRUDERS; e++)
EEPROM_READ_VAR(i, filament_size[e]);
#if ENABLED(IDLE_OOZING_PREVENT)
EEPROM_READ_VAR(i, IDLE_OOZING_enabled);
#endif
#if MB(ALLIGATOR)
EEPROM_READ_VAR(i, motor_current);
#endif
if (eeprom_checksum == stored_checksum) {
Config_Postprocess();
ECHO_SV(DB, version);
ECHO_MV(" stored settings retrieved (", i);
ECHO_EM(" bytes)");
}
else {
ECHO_LM(ER, "EEPROM checksum mismatch");
Config_ResetDefault();
}
}
#if ENABLED(EEPROM_CHITCHAT)
Config_PrintSettings();
#endif
}
/**
* G28: Home all axes according to settings
*
* Parameters
*
* None Home to all axes with no parameters.
* With QUICK_HOME enabled XY will home together, then Z.
*
* O Home only if position is unknown
*
* Rn Raise by n mm/inches before homing
*
* Cartesian/SCARA parameters
*
* X Home to the X endstop
* Y Home to the Y endstop
* Z Home to the Z endstop
*
*/
void GcodeSuite::G28(const bool always_home_all) {
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
SERIAL_ECHOLNPGM(">>> G28");
log_machine_info();
}
#endif
#if ENABLED(DUAL_X_CARRIAGE)
bool IDEX_saved_duplication_state = extruder_duplication_enabled;
DualXMode IDEX_saved_mode = dual_x_carriage_mode;
#endif
#if ENABLED(MARLIN_DEV_MODE)
if (parser.seen('S')) {
LOOP_XYZ(a) set_axis_is_at_home((AxisEnum)a);
sync_plan_position();
SERIAL_ECHOLNPGM("Simulated Homing");
report_current_position();
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< G28");
#endif
return;
}
#endif
if (parser.boolval('O')) {
if (
#if ENABLED(HOME_AFTER_DEACTIVATE)
all_axes_known() // homing needed anytime steppers deactivate
#else
all_axes_homed() // homing needed only if never homed
#endif
) {
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) {
SERIAL_ECHOLNPGM("> homing not needed, skip");
SERIAL_ECHOLNPGM("<<< G28");
}
#endif
return;
}
}
// Wait for planner moves to finish!
planner.synchronize();
// Disable the leveling matrix before homing
#if HAS_LEVELING
// Cancel the active G29 session
#if ENABLED(PROBE_MANUALLY)
g29_in_progress = false;
#endif
#if ENABLED(RESTORE_LEVELING_AFTER_G28)
const bool leveling_was_active = planner.leveling_active;
#endif
set_bed_leveling_enabled(false);
#endif
#if ENABLED(CNC_WORKSPACE_PLANES)
workspace_plane = PLANE_XY;
#endif
#if ENABLED(BLTOUCH)
bltouch_init();
#endif
#if ENABLED(IMPROVE_HOMING_RELIABILITY)
slow_homing_t slow_homing{0};
slow_homing.acceleration.x = planner.settings.max_acceleration_mm_per_s2[X_AXIS];
slow_homing.acceleration.y = planner.settings.max_acceleration_mm_per_s2[Y_AXIS];
slow_homing.jerk.x = planner.max_jerk[X_AXIS];
slow_homing.jerk.y = planner.max_jerk[Y_AXIS];
planner.settings.max_acceleration_mm_per_s2[X_AXIS] = 100;
planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = 100;
planner.max_jerk[X_AXIS] = 0;
planner.max_jerk[Y_AXIS] = 0;
// steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
planner.reset_acceleration_rates();
#endif
// Always home with tool 0 active
#if HOTENDS > 1
#if DISABLED(DELTA) || ENABLED(DELTA_HOME_TO_SAFE_ZONE)
const uint8_t old_tool_index = active_extruder;
#endif
tool_change(0, 0, true);
#endif
#if ENABLED(DUAL_X_CARRIAGE) || ENABLED(DUAL_NOZZLE_DUPLICATION_MODE)
extruder_duplication_enabled = false;
#endif
setup_for_endstop_or_probe_move();
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("> endstops.enable(true)");
#endif
endstops.enable(true); // Enable endstops for next homing move
#if ENABLED(DELTA)
home_delta();
UNUSED(always_home_all);
#else // NOT DELTA
const bool homeX = always_home_all || parser.seen('X'),
homeY = always_home_all || parser.seen('Y'),
homeZ = always_home_all || parser.seen('Z'),
home_all = (!homeX && !homeY && !homeZ) || (homeX && homeY && homeZ);
set_destination_from_current();
#if Z_HOME_DIR > 0 // If homing away from BED do Z first
if (home_all || homeZ) homeaxis(Z_AXIS);
#endif
const float z_homing_height = (
#if ENABLED(UNKNOWN_Z_NO_RAISE)
!TEST(axis_known_position, Z_AXIS) ? 0 :
#endif
(parser.seenval('R') ? parser.value_linear_units() : Z_HOMING_HEIGHT)
);
if (z_homing_height && (home_all || homeX || homeY)) {
// Raise Z before homing any other axes and z is not already high enough (never lower z)
destination[Z_AXIS] = z_homing_height;
if (destination[Z_AXIS] > current_position[Z_AXIS]) {
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING))
SERIAL_ECHOLNPAIR("Raise Z (before homing) to ", destination[Z_AXIS]);
#endif
do_blocking_move_to_z(destination[Z_AXIS]);
}
}
#if ENABLED(QUICK_HOME)
if (home_all || (homeX && homeY)) quick_home_xy();
#endif
// Home Y (before X)
#if ENABLED(HOME_Y_BEFORE_X)
if (home_all || homeY
#if ENABLED(CODEPENDENT_XY_HOMING)
|| homeX
#endif
) homeaxis(Y_AXIS);
#endif
// Home X
if (home_all || homeX
#if ENABLED(CODEPENDENT_XY_HOMING) && DISABLED(HOME_Y_BEFORE_X)
|| homeY
#endif
) {
#if ENABLED(DUAL_X_CARRIAGE)
// Always home the 2nd (right) extruder first
active_extruder = 1;
homeaxis(X_AXIS);
// Remember this extruder's position for later tool change
inactive_extruder_x_pos = current_position[X_AXIS];
// Home the 1st (left) extruder
active_extruder = 0;
homeaxis(X_AXIS);
// Consider the active extruder to be parked
COPY(raised_parked_position, current_position);
delayed_move_time = 0;
active_extruder_parked = true;
#else
homeaxis(X_AXIS);
#endif
}
// Home Y (after X)
#if DISABLED(HOME_Y_BEFORE_X)
if (home_all || homeY) homeaxis(Y_AXIS);
#endif
// Home Z last if homing towards the bed
#if Z_HOME_DIR < 0
if (home_all || homeZ) {
#if ENABLED(Z_SAFE_HOMING)
home_z_safely();
#else
homeaxis(Z_AXIS);
#endif
#if HOMING_Z_WITH_PROBE && defined(Z_AFTER_PROBING)
move_z_after_probing();
#endif
} // home_all || homeZ
#endif // Z_HOME_DIR < 0
sync_plan_position();
#endif // !DELTA (G28)
/**
* Preserve DXC mode across a G28 for IDEX printers in DXC_DUPLICATION_MODE.
* This is important because it lets a user use the LCD Panel to set an IDEX Duplication mode, and
* then print a standard GCode file that contains a single print that does a G28 and has no other
* IDEX specific commands in it.
*/
#if ENABLED(DUAL_X_CARRIAGE)
if (dxc_is_duplicating()) {
// Always home the 2nd (right) extruder first
active_extruder = 1;
homeaxis(X_AXIS);
// Remember this extruder's position for later tool change
inactive_extruder_x_pos = current_position[X_AXIS];
// Home the 1st (left) extruder
active_extruder = 0;
homeaxis(X_AXIS);
// Consider the active extruder to be parked
COPY(raised_parked_position, current_position);
delayed_move_time = 0;
active_extruder_parked = true;
extruder_duplication_enabled = IDEX_saved_duplication_state;
extruder_duplication_enabled = false;
dual_x_carriage_mode = IDEX_saved_mode;
stepper.set_directions();
}
#endif // DUAL_X_CARRIAGE
endstops.not_homing();
#if ENABLED(DELTA) && ENABLED(DELTA_HOME_TO_SAFE_ZONE)
// move to a height where we can use the full xy-area
do_blocking_move_to_z(delta_clip_start_height);
#endif
#if HAS_LEVELING && ENABLED(RESTORE_LEVELING_AFTER_G28)
set_bed_leveling_enabled(leveling_was_active);
#endif
clean_up_after_endstop_or_probe_move();
// Restore the active tool after homing
#if HOTENDS > 1 && (DISABLED(DELTA) || ENABLED(DELTA_HOME_TO_SAFE_ZONE))
#if ENABLED(PARKING_EXTRUDER)
#define NO_FETCH false // fetch the previous toolhead
#else
#define NO_FETCH true
#endif
tool_change(old_tool_index, 0, NO_FETCH);
#endif
#if ENABLED(IMPROVE_HOMING_RELIABILITY)
planner.settings.max_acceleration_mm_per_s2[X_AXIS] = slow_homing.acceleration.x;
planner.settings.max_acceleration_mm_per_s2[Y_AXIS] = slow_homing.acceleration.y;
planner.max_jerk[X_AXIS] = slow_homing.jerk.x;
planner.max_jerk[Y_AXIS] = slow_homing.jerk.y;
// steps per sq second need to be updated to agree with the units per sq second (as they are what is used in the planner)
planner.reset_acceleration_rates();
#endif
ui.refresh();
report_current_position();
#if ENABLED(NANODLP_Z_SYNC)
#if ENABLED(NANODLP_ALL_AXIS)
#define _HOME_SYNC true // For any axis, output sync text.
#else
#define _HOME_SYNC (home_all || homeZ) // Only for Z-axis
#endif
if (_HOME_SYNC)
SERIAL_ECHOLNPGM(MSG_Z_MOVE_COMP);
#endif
#if ENABLED(DEBUG_LEVELING_FEATURE)
if (DEBUGGING(LEVELING)) SERIAL_ECHOLNPGM("<<< G28");
#endif
#if HAS_DRIVER(L6470)
// Set L6470 absolute position registers to counts
for (uint8_t j = 1; j <= L6470::chain[0]; j++) {
const uint8_t cv = L6470::chain[j];
L6470.set_param(cv, L6470_ABS_POS, stepper.position((AxisEnum)L6470.axis_xref[cv]));
}
#endif
}
DEBUG_NO_STATIC int
pfkey_address_parse(struct sadb_ext *pfkey_ext)
{
int error = 0;
int saddr_len = 0;
struct sadb_address *pfkey_address = (struct sadb_address *)pfkey_ext;
struct sockaddr* s = (struct sockaddr*)((char*)pfkey_address + sizeof(*pfkey_address));
char ipaddr_txt[ADDRTOT_BUF];
DEBUGGING(PF_KEY_DEBUG_PARSE_FLOW,
"pfkey_address_parse:enter\n");
/* sanity checks... */
if(!pfkey_address) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_address_parse: "
"NULL pointer passed in.\n");
SENDERR(EINVAL);
}
if(pfkey_address->sadb_address_len <
(sizeof(struct sadb_address) + sizeof(struct sockaddr))/
IPSEC_PFKEYv2_ALIGN) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_address_parse: "
"size wrong 1 ext_len=%d, adr_ext_len=%ld, saddr_len=%ld.\n",
pfkey_address->sadb_address_len,
sizeof(struct sadb_address),
sizeof(struct sockaddr));
SENDERR(EINVAL);
}
if(pfkey_address->sadb_address_reserved) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_address_parse: "
"res=%d, must be zero.\n",
pfkey_address->sadb_address_reserved);
SENDERR(EINVAL);
}
switch(pfkey_address->sadb_address_exttype) {
case SADB_EXT_ADDRESS_SRC:
case SADB_EXT_ADDRESS_DST:
case SADB_EXT_ADDRESS_PROXY:
case SADB_X_EXT_ADDRESS_DST2:
case SADB_X_EXT_ADDRESS_SRC_FLOW:
case SADB_X_EXT_ADDRESS_DST_FLOW:
case SADB_X_EXT_ADDRESS_SRC_MASK:
case SADB_X_EXT_ADDRESS_DST_MASK:
#ifdef NAT_TRAVERSAL
case SADB_X_EXT_NAT_T_OA:
#endif
break;
default:
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_address_parse: "
"unexpected ext_type=%d.\n",
pfkey_address->sadb_address_exttype);
SENDERR(EINVAL);
}
switch(s->sa_family) {
case AF_INET:
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_address_parse: "
"found address family=%d, AF_INET.\n",
s->sa_family);
saddr_len = sizeof(struct sockaddr_in);
sprintf(ipaddr_txt, "%d.%d.%d.%d"
, (((struct sockaddr_in*)s)->sin_addr.s_addr >> 0) & 0xFF
, (((struct sockaddr_in*)s)->sin_addr.s_addr >> 8) & 0xFF
, (((struct sockaddr_in*)s)->sin_addr.s_addr >> 16) & 0xFF
, (((struct sockaddr_in*)s)->sin_addr.s_addr >> 24) & 0xFF);
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_address_parse: "
"found address=%s.\n",
ipaddr_txt);
break;
case AF_INET6:
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_address_parse: "
"found address family=%d, AF_INET6.\n",
s->sa_family);
saddr_len = sizeof(struct sockaddr_in6);
sprintf(ipaddr_txt, "%x:%x:%x:%x:%x:%x:%x:%x"
, ntohs(((struct sockaddr_in6*)s)->sin6_addr.s6_addr16[0])
, ntohs(((struct sockaddr_in6*)s)->sin6_addr.s6_addr16[1])
, ntohs(((struct sockaddr_in6*)s)->sin6_addr.s6_addr16[2])
, ntohs(((struct sockaddr_in6*)s)->sin6_addr.s6_addr16[3])
, ntohs(((struct sockaddr_in6*)s)->sin6_addr.s6_addr16[4])
, ntohs(((struct sockaddr_in6*)s)->sin6_addr.s6_addr16[5])
, ntohs(((struct sockaddr_in6*)s)->sin6_addr.s6_addr16[6])
, ntohs(((struct sockaddr_in6*)s)->sin6_addr.s6_addr16[7]));
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_address_parse: "
"found address=%s.\n",
ipaddr_txt);
break;
default:
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_address_parse: "
"s->sa_family=%d not supported.\n",
s->sa_family);
SENDERR(EPFNOSUPPORT);
}
if(pfkey_address->sadb_address_len !=
DIVUP(sizeof(struct sadb_address) + saddr_len, IPSEC_PFKEYv2_ALIGN)) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_address_parse: "
"size wrong 2 ext_len=%d, adr_ext_len=%ld, saddr_len=%d.\n",
pfkey_address->sadb_address_len,
sizeof(struct sadb_address),
saddr_len);
SENDERR(EINVAL);
}
if(pfkey_address->sadb_address_prefixlen != 0) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_address_parse: "
"address prefixes not supported yet.\n");
SENDERR(EAFNOSUPPORT); /* not supported yet */
}
/* XXX check if port!=0 */
DEBUGGING(PF_KEY_DEBUG_PARSE_FLOW,
"pfkey_address_parse: successful.\n");
errlab:
return error;
}
inline void magnetic_parking_extruder_tool_change(const uint8_t tmp_extruder) {
const float oldx = current_position[X_AXIS],
grabpos = mpe_settings.parking_xpos[tmp_extruder] + (tmp_extruder ? mpe_settings.grab_distance : -mpe_settings.grab_distance),
offsetcompensation =
#if HAS_HOTEND_OFFSET
hotend_offset[X_AXIS][active_extruder] * mpe_settings.compensation_factor
#else
0
#endif
;
if (axis_unhomed_error(true, false, false)) return;
/**
* Z Lift and Nozzle Offset shift ar defined in caller method to work equal with any Multi Hotend realization
*
* Steps:
* 1. Move high speed to park position of new extruder
* 2. Move to couple position of new extruder (this also discouple the old extruder)
* 3. Move to park position of new extruder
* 4. Move high speed to approach park position of old extruder
* 5. Move to park position of old extruder
* 6. Move to starting position
*/
// STEP 1
current_position[X_AXIS] = mpe_settings.parking_xpos[tmp_extruder] + offsetcompensation;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR("(1) Move extruder ", int(tmp_extruder));
DEBUG_POS(" to new extruder ParkPos", current_position);
}
planner.buffer_line(current_position, mpe_settings.fast_feedrate, tmp_extruder);
planner.synchronize();
// STEP 2
current_position[X_AXIS] = grabpos + offsetcompensation;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR("(2) Couple extruder ", int(tmp_extruder));
DEBUG_POS(" to new extruder GrabPos", current_position);
}
planner.buffer_line(current_position, mpe_settings.slow_feedrate, tmp_extruder);
planner.synchronize();
// Delay before moving tool, to allow magnetic coupling
gcode.dwell(150);
// STEP 3
current_position[X_AXIS] = mpe_settings.parking_xpos[tmp_extruder] + offsetcompensation;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR("(3) Move extruder ", int(tmp_extruder));
DEBUG_POS(" back to new extruder ParkPos", current_position);
}
planner.buffer_line(current_position, mpe_settings.slow_feedrate, tmp_extruder);
planner.synchronize();
// STEP 4
current_position[X_AXIS] = mpe_settings.parking_xpos[active_extruder] + (active_extruder == 0 ? MPE_TRAVEL_DISTANCE : -MPE_TRAVEL_DISTANCE) + offsetcompensation;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR("(4) Move extruder ", int(tmp_extruder));
DEBUG_POS(" close to old extruder ParkPos", current_position);
}
planner.buffer_line(current_position, mpe_settings.fast_feedrate, tmp_extruder);
planner.synchronize();
// STEP 5
current_position[X_AXIS] = mpe_settings.parking_xpos[active_extruder] + offsetcompensation;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR("(5) Park extruder ", int(tmp_extruder));
DEBUG_POS(" at old extruder ParkPos", current_position);
}
planner.buffer_line(current_position, mpe_settings.slow_feedrate, tmp_extruder);
planner.synchronize();
// STEP 6
current_position[X_AXIS] = oldx;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOPAIR("(6) Move extruder ", int(tmp_extruder));
DEBUG_POS(" to starting position", current_position);
}
planner.buffer_line(current_position, mpe_settings.fast_feedrate, tmp_extruder);
planner.synchronize();
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Autopark done.");
}
DEBUG_NO_STATIC int
pfkey_key_parse(struct sadb_ext *pfkey_ext)
{
int error = 0;
struct sadb_key *pfkey_key = (struct sadb_key *)pfkey_ext;
DEBUGGING(PF_KEY_DEBUG_PARSE_FLOW,
"pfkey_key_parse:enter\n");
/* sanity checks... */
if(!pfkey_key) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_key_parse: "
"NULL pointer passed in.\n");
SENDERR(EINVAL);
}
if(pfkey_key->sadb_key_len < sizeof(struct sadb_key) / IPSEC_PFKEYv2_ALIGN) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_key_parse: "
"size wrong ext_len=%d, key_ext_len=%ld.\n",
pfkey_key->sadb_key_len,
sizeof(struct sadb_key));
SENDERR(EINVAL);
}
if(!pfkey_key->sadb_key_bits) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_key_parse: "
"key length set to zero, must be non-zero.\n");
SENDERR(EINVAL);
}
if(pfkey_key->sadb_key_len !=
DIVUP(sizeof(struct sadb_key) * OCTETBITS + pfkey_key->sadb_key_bits,
PFKEYBITS)) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_key_parse: "
"key length=%d does not agree with extension length=%d.\n",
pfkey_key->sadb_key_bits,
pfkey_key->sadb_key_len);
SENDERR(EINVAL);
}
if(pfkey_key->sadb_key_reserved) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_key_parse: "
"res=%d, must be zero.\n",
pfkey_key->sadb_key_reserved);
SENDERR(EINVAL);
}
if(! ( (pfkey_key->sadb_key_exttype == SADB_EXT_KEY_AUTH) ||
(pfkey_key->sadb_key_exttype == SADB_EXT_KEY_ENCRYPT))) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_key_parse: "
"expecting extension type AUTH or ENCRYPT, got %d.\n",
pfkey_key->sadb_key_exttype);
SENDERR(EINVAL);
}
DEBUGGING(PF_KEY_DEBUG_PARSE_STRUCT,
"pfkey_key_parse: "
"success, found len=%d exttype=%d bits=%d reserved=%d.\n",
pfkey_key->sadb_key_len,
pfkey_key->sadb_key_exttype,
pfkey_key->sadb_key_bits,
pfkey_key->sadb_key_reserved);
errlab:
return error;
}
inline void switching_toolhead_tool_change(const uint8_t tmp_extruder, const float fr_mm_s/*=0.0*/, bool no_move/*=false*/) {
if (no_move) return;
constexpr uint16_t angles[2] = SWITCHING_TOOLHEAD_SERVO_ANGLES;
const float toolheadposx[] = SWITCHING_TOOLHEAD_X_POS,
placexpos = toolheadposx[active_extruder],
grabxpos = toolheadposx[tmp_extruder];
/**
* 1. Raise Z to give enough clearance
* 2. Move to switch position of current toolhead
* 3. Unlock tool and drop it in the dock
* 4. Move to the new toolhead
* 5. Grab and lock the new toolhead
*/
// 1. Raise Z to give enough clearance
if (DEBUGGING(LEVELING)) DEBUG_POS("Starting Toolhead change", current_position);
current_position[Z_AXIS] += toolchange_settings.z_raise;
if (DEBUGGING(LEVELING)) DEBUG_POS("(1) Raise Z-Axis", current_position);
fast_line_to_current(Z_AXIS);
planner.synchronize();
// 2. Move to switch position of current toolhead
current_position[X_AXIS] = placexpos;
if (DEBUGGING(LEVELING)) {
DEBUG_ECHOLNPAIR("(2) Place old tool ", int(active_extruder));
DEBUG_POS("Move X SwitchPos", current_position);
}
fast_line_to_current(X_AXIS);
planner.synchronize();
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - SWITCHING_TOOLHEAD_Y_SECURITY;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos + Security", current_position);
fast_line_to_current(Y_AXIS);
planner.synchronize();
// 3. Unlock tool and drop it in the dock
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(3) Unlock and Place Toolhead");
MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, angles[1]);
safe_delay(500);
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos", current_position);
planner.buffer_line(current_position,(planner.settings.max_feedrate_mm_s[Y_AXIS] * 0.5), active_extruder);
planner.synchronize();
safe_delay(200);
current_position[Y_AXIS] -= SWITCHING_TOOLHEAD_Y_CLEAR;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position);
fast_line_to_current(Y_AXIS); // move away from docked toolhead
planner.synchronize();
// 4. Move to the new toolhead
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(4) Move to new toolhead position");
current_position[X_AXIS] = grabxpos;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move to new toolhead X", current_position);
fast_line_to_current(X_AXIS);
planner.synchronize();
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS - SWITCHING_TOOLHEAD_Y_SECURITY;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos + Security", current_position);
fast_line_to_current(Y_AXIS);
planner.synchronize();
// 5. Grab and lock the new toolhead
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("(5) Grab and lock new toolhead ");
current_position[Y_AXIS] = SWITCHING_TOOLHEAD_Y_POS;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move Y SwitchPos", current_position);
planner.buffer_line(current_position, planner.settings.max_feedrate_mm_s[Y_AXIS] * 0.5, active_extruder);
planner.synchronize();
safe_delay(200);
MOVE_SERVO(SWITCHING_TOOLHEAD_SERVO_NR, angles[0]);
safe_delay(500);
current_position[Y_AXIS] -= SWITCHING_TOOLHEAD_Y_CLEAR;
if (DEBUGGING(LEVELING)) DEBUG_POS("Move back Y clear", current_position);
fast_line_to_current(Y_AXIS); // move away from docked toolhead
planner.synchronize();
if (DEBUGGING(LEVELING)) DEBUG_ECHOLNPGM("Toolhead change done.");
}
DEBUG_NO_STATIC int
pfkey_prop_parse(struct sadb_ext *pfkey_ext)
{
int error = 0;
int i, num_comb;
struct sadb_prop *pfkey_prop = (struct sadb_prop *)pfkey_ext;
struct sadb_comb *pfkey_comb = (struct sadb_comb *)((char*)pfkey_ext + sizeof(struct sadb_prop));
/* sanity checks... */
if((pfkey_prop->sadb_prop_len < sizeof(struct sadb_prop) / IPSEC_PFKEYv2_ALIGN) ||
(((pfkey_prop->sadb_prop_len * IPSEC_PFKEYv2_ALIGN) - sizeof(struct sadb_prop)) % sizeof(struct sadb_comb))) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"size wrong ext_len=%d, prop_ext_len=%ld comb_ext_len=%ld.\n",
pfkey_prop->sadb_prop_len,
sizeof(struct sadb_prop),
sizeof(struct sadb_comb));
SENDERR(EINVAL);
}
if(pfkey_prop->sadb_prop_replay > 64) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"replay window size: %d -- must be 0 <= size <= 64\n",
pfkey_prop->sadb_prop_replay);
SENDERR(EINVAL);
}
for(i=0; i<3; i++) {
if(pfkey_prop->sadb_prop_reserved[i]) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"res[%d]=%d, must be zero.\n",
i, pfkey_prop->sadb_prop_reserved[i]);
SENDERR(EINVAL);
}
}
num_comb = ((pfkey_prop->sadb_prop_len * IPSEC_PFKEYv2_ALIGN) - sizeof(struct sadb_prop)) / sizeof(struct sadb_comb);
for(i = 0; i < num_comb; i++) {
if(pfkey_comb->sadb_comb_auth > SADB_AALG_MAX) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_auth=%d > SADB_AALG_MAX=%d.\n",
i,
pfkey_comb->sadb_comb_auth,
SADB_AALG_MAX);
SENDERR(EINVAL);
}
if(pfkey_comb->sadb_comb_auth) {
if(!pfkey_comb->sadb_comb_auth_minbits) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_auth_minbits=0, fatal.\n",
i);
SENDERR(EINVAL);
}
if(!pfkey_comb->sadb_comb_auth_maxbits) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_auth_maxbits=0, fatal.\n",
i);
SENDERR(EINVAL);
}
if(pfkey_comb->sadb_comb_auth_minbits > pfkey_comb->sadb_comb_auth_maxbits) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_auth_minbits=%d > maxbits=%d, fatal.\n",
i,
pfkey_comb->sadb_comb_auth_minbits,
pfkey_comb->sadb_comb_auth_maxbits);
SENDERR(EINVAL);
}
} else {
if(pfkey_comb->sadb_comb_auth_minbits) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_auth_minbits=%d != 0, fatal.\n",
i,
pfkey_comb->sadb_comb_auth_minbits);
SENDERR(EINVAL);
}
if(pfkey_comb->sadb_comb_auth_maxbits) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_auth_maxbits=%d != 0, fatal.\n",
i,
pfkey_comb->sadb_comb_auth_maxbits);
SENDERR(EINVAL);
}
}
if(pfkey_comb->sadb_comb_encrypt > SADB_EALG_MAX) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_comb_parse: "
"pfkey_comb[%d]->sadb_comb_encrypt=%d > SADB_EALG_MAX=%d.\n",
i,
pfkey_comb->sadb_comb_encrypt,
SADB_EALG_MAX);
SENDERR(EINVAL);
}
if(pfkey_comb->sadb_comb_encrypt) {
if(!pfkey_comb->sadb_comb_encrypt_minbits) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_encrypt_minbits=0, fatal.\n",
i);
SENDERR(EINVAL);
}
if(!pfkey_comb->sadb_comb_encrypt_maxbits) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_encrypt_maxbits=0, fatal.\n",
i);
SENDERR(EINVAL);
}
if(pfkey_comb->sadb_comb_encrypt_minbits > pfkey_comb->sadb_comb_encrypt_maxbits) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_encrypt_minbits=%d > maxbits=%d, fatal.\n",
i,
pfkey_comb->sadb_comb_encrypt_minbits,
pfkey_comb->sadb_comb_encrypt_maxbits);
SENDERR(EINVAL);
}
} else {
if(pfkey_comb->sadb_comb_encrypt_minbits) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_encrypt_minbits=%d != 0, fatal.\n",
i,
pfkey_comb->sadb_comb_encrypt_minbits);
SENDERR(EINVAL);
}
if(pfkey_comb->sadb_comb_encrypt_maxbits) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_encrypt_maxbits=%d != 0, fatal.\n",
i,
pfkey_comb->sadb_comb_encrypt_maxbits);
SENDERR(EINVAL);
}
}
/* XXX do sanity check on flags */
if(pfkey_comb->sadb_comb_hard_allocations && pfkey_comb->sadb_comb_soft_allocations > pfkey_comb->sadb_comb_hard_allocations) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_soft_allocations=%d > hard_allocations=%d, fatal.\n",
i,
pfkey_comb->sadb_comb_soft_allocations,
pfkey_comb->sadb_comb_hard_allocations);
SENDERR(EINVAL);
}
if(pfkey_comb->sadb_comb_hard_bytes && pfkey_comb->sadb_comb_soft_bytes > pfkey_comb->sadb_comb_hard_bytes) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_soft_bytes=%Ld > hard_bytes=%Ld, fatal.\n",
i,
pfkey_comb->sadb_comb_soft_bytes,
pfkey_comb->sadb_comb_hard_bytes);
SENDERR(EINVAL);
}
if(pfkey_comb->sadb_comb_hard_addtime && pfkey_comb->sadb_comb_soft_addtime > pfkey_comb->sadb_comb_hard_addtime) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_soft_addtime=%Ld > hard_addtime=%Ld, fatal.\n",
i,
pfkey_comb->sadb_comb_soft_addtime,
pfkey_comb->sadb_comb_hard_addtime);
SENDERR(EINVAL);
}
if(pfkey_comb->sadb_comb_hard_usetime && pfkey_comb->sadb_comb_soft_usetime > pfkey_comb->sadb_comb_hard_usetime) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_comb_soft_usetime=%Ld > hard_usetime=%Ld, fatal.\n",
i,
pfkey_comb->sadb_comb_soft_usetime,
pfkey_comb->sadb_comb_hard_usetime);
SENDERR(EINVAL);
}
if(pfkey_comb->sadb_x_comb_hard_packets && pfkey_comb->sadb_x_comb_soft_packets > pfkey_comb->sadb_x_comb_hard_packets) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"pfkey_comb[%d]->sadb_x_comb_soft_packets=%d > hard_packets=%d, fatal.\n",
i,
pfkey_comb->sadb_x_comb_soft_packets,
pfkey_comb->sadb_x_comb_hard_packets);
SENDERR(EINVAL);
}
if(pfkey_comb->sadb_comb_reserved) {
DEBUGGING(PF_KEY_DEBUG_PARSE_PROBLEM,
"pfkey_prop_parse: "
"comb[%d].res=%d, must be zero.\n",
i,
pfkey_comb->sadb_comb_reserved);
SENDERR(EINVAL);
}
pfkey_comb++;
}
errlab:
return error;
}
void Stopwatch::debug(const char func[]) {
if (DEBUGGING(INFO)) {
SERIAL_MV("Stopwatch:", func);
SERIAL_EM("()");
}
}
void mdvi_shrink_glyph(DviContext *dvi, DviFont *font,
DviFontChar *pk, DviGlyph *dest)
{
int rows_left, rows, init_cols;
int cols_left, cols;
BmUnit *old_ptr, *new_ptr;
BITMAP *oldmap, *newmap;
BmUnit m, *cp;
DviGlyph *glyph;
int sample, min_sample;
int old_stride;
int new_stride;
int x, y;
int w, h;
int hs, vs;
hs = dvi->params.hshrink;
vs = dvi->params.vshrink;
min_sample = vs * hs * dvi->params.density / 100;
glyph = &pk->glyph;
oldmap = (BITMAP *)glyph->data;
x = (int)glyph->x / hs;
init_cols = (int)glyph->x - x * hs;
if(init_cols <= 0)
init_cols += hs;
else
x++;
w = x + ROUND((int)glyph->w - glyph->x, hs);
cols = (int)glyph->y + 1;
y = cols / vs;
rows = cols - y * vs;
if(rows <= 0) {
rows += vs;
y--;
}
h = y + ROUND((int)glyph->h - cols, vs) + 1;
/* create the new glyph */
newmap = bitmap_alloc(w, h);
dest->data = newmap;
dest->x = x;
dest->y = glyph->y / vs;
dest->w = w;
dest->h = h;
old_ptr = oldmap->data;
old_stride = oldmap->stride;
new_ptr = newmap->data;
new_stride = newmap->stride;
rows_left = glyph->h;
while(rows_left) {
if(rows > rows_left)
rows = rows_left;
cols_left = glyph->w;
m = FIRSTMASK;
cp = new_ptr;
cols = init_cols;
while(cols_left > 0) {
if(cols > cols_left)
cols = cols_left;
sample = do_sample(old_ptr, old_stride,
glyph->w - cols_left, cols, rows);
if(sample >= min_sample)
*cp |= m;
if(m == LASTMASK) {
m = FIRSTMASK;
cp++;
} else
NEXTMASK(m);
cols_left -= cols;
cols = hs;
}
new_ptr = bm_offset(new_ptr, new_stride);
old_ptr = bm_offset(old_ptr, rows * old_stride);
rows_left -= rows;
rows = vs;
}
DEBUG((DBG_BITMAPS, "shrink_glyph: (%dw,%dh,%dx,%dy) -> (%dw,%dh,%dx,%dy)\n",
glyph->w, glyph->h, glyph->x, glyph->y,
dest->w, dest->h, dest->x, dest->y));
if(DEBUGGING(BITMAP_DATA))
bitmap_print(stderr, newmap);
}