Exemple #1
0
/**
 * - 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;
}
Exemple #4
0
// 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;
}
Exemple #5
0
/**
 * 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;
}	
Exemple #8
0
  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);
  }
Exemple #9
0
  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;
}
Exemple #11
0
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;
}	
Exemple #12
0
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
}
Exemple #14
0
/**
 * 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;
}
Exemple #16
0
  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;
}
Exemple #18
0
  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;
}
Exemple #20
0
 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);
}