void taskUpdateBattery(void)
{
static uint32_t vbatLastServiced = 0;
static uint32_t ibatLastServiced = 0;
if (feature(FEATURE_VBAT)) {
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
voltageMeterUpdate();
batteryUpdate();
}
}
if (feature(FEATURE_AMPERAGE_METER)) {
int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime;
if (batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC) {
amperageUpdateMeter(ibatTimeSinceLastServiced);
} else {
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), rcControlsConfig()->deadband3d_throttle);
bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP));
int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
amperageUpdateVirtualMeter(ibatTimeSinceLastServiced, ARMING_FLAG(ARMED), throttleLowAndMotorStop, throttleOffset);
}
}
}
}
/**
* Method entry for static native methods:
* int java.util.zip.CRC32.updateBytes(int crc, byte[] b, int off, int len)
* int java.util.zip.CRC32.updateByteBuffer(int crc, long buf, int off, int len)
*/
address InterpreterGenerator::generate_CRC32_updateBytes_entry(AbstractInterpreter::MethodKind kind) {
if (UseCRC32Intrinsics) {
address entry = __ pc();
// rbx,: Method*
// rsi: senderSP must preserved for slow path, set SP to it on fast path
// rdx: scratch
// rdi: scratch
Label slow_path;
// If we need a safepoint check, generate full interpreter entry.
ExternalAddress state(SafepointSynchronize::address_of_state());
__ cmp32(ExternalAddress(SafepointSynchronize::address_of_state()),
SafepointSynchronize::_not_synchronized);
__ jcc(Assembler::notEqual, slow_path);
// We don't generate local frame and don't align stack because
// we call stub code and there is no safepoint on this path.
// Load parameters
const Register crc = rax; // crc
const Register buf = rdx; // source java byte array address
const Register len = rdi; // length
// value x86_32
// interp. arg ptr ESP + 4
// int java.util.zip.CRC32.updateBytes(int crc, byte[] b, int off, int len)
// 3 2 1 0
// int java.util.zip.CRC32.updateByteBuffer(int crc, long buf, int off, int len)
// 4 2,3 1 0
// Arguments are reversed on java expression stack
__ movl(len, Address(rsp, 4 + 0)); // Length
// Calculate address of start element
if (kind == Interpreter::java_util_zip_CRC32_updateByteBuffer) {
__ movptr(buf, Address(rsp, 4 + 2 * wordSize)); // long buf
__ addptr(buf, Address(rsp, 4 + 1 * wordSize)); // + offset
__ movl(crc, Address(rsp, 4 + 4 * wordSize)); // Initial CRC
} else {
__ movptr(buf, Address(rsp, 4 + 2 * wordSize)); // byte[] array
__ addptr(buf, arrayOopDesc::base_offset_in_bytes(T_BYTE)); // + header size
__ addptr(buf, Address(rsp, 4 + 1 * wordSize)); // + offset
__ movl(crc, Address(rsp, 4 + 3 * wordSize)); // Initial CRC
}
__ super_call_VM_leaf(CAST_FROM_FN_PTR(address, StubRoutines::updateBytesCRC32()), crc, buf, len);
// result in rax
// _areturn
__ pop(rdi); // get return address
__ mov(rsp, rsi); // set sp to sender sp
__ jmp(rdi);
// generate a vanilla native entry as the slow path
__ bind(slow_path);
__ jump_to_entry(Interpreter::entry_for_kind(Interpreter::native));
return entry;
}
return NULL;
}
address InterpreterGenerator::generate_empty_entry(void) {
// rbx,: methodOop
// rcx: receiver (unused)
// rsi: previous interpreter state (C++ interpreter) must preserve
// rsi: sender sp must set sp to this value on return
if (!UseFastEmptyMethods) return NULL;
address entry_point = __ pc();
// If we need a safepoint check, generate full interpreter entry.
Label slow_path;
ExternalAddress state(SafepointSynchronize::address_of_state());
__ cmp32(ExternalAddress(SafepointSynchronize::address_of_state()),
SafepointSynchronize::_not_synchronized);
__ jcc(Assembler::notEqual, slow_path);
// do nothing for empty methods (do not even increment invocation counter)
// Code: _return
// _return
// return w/o popping parameters
__ pop(rax);
__ mov(rsp, rsi);
__ jmp(rax);
__ bind(slow_path);
(void) generate_normal_entry(false);
return entry_point;
}
void taskUpdateBattery(void)
{
static uint32_t vbatLastServiced = 0;
static uint32_t ibatLastServiced = 0;
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
updateBattery();
}
int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime;
updateCurrentMeter(ibatTimeSinceLastServiced);
}
}
static void batteryUpdateLVC(timeUs_t currentTimeUs)
{
if (batteryConfig()->lvcPercentage < 100) {
if (voltageState == BATTERY_CRITICAL && !lowVoltageCutoff.enabled) {
lowVoltageCutoff.enabled = true;
lowVoltageCutoff.startTime = currentTimeUs;
lowVoltageCutoff.percentage = 100;
}
if (lowVoltageCutoff.enabled) {
if (cmp32(currentTimeUs,lowVoltageCutoff.startTime) < LVC_AFFECT_TIME) {
lowVoltageCutoff.percentage = 100 - (cmp32(currentTimeUs,lowVoltageCutoff.startTime) * (100 - batteryConfig()->lvcPercentage) / LVC_AFFECT_TIME);
}
else {
lowVoltageCutoff.percentage = batteryConfig()->lvcPercentage;
}
}
}
}
/**
* Method entry for static native methods:
* int java.util.zip.CRC32.updateBytes(int crc, byte[] b, int off, int len)
* int java.util.zip.CRC32.updateByteBuffer(int crc, long buf, int off, int len)
*/
address TemplateInterpreterGenerator::generate_CRC32_updateBytes_entry(AbstractInterpreter::MethodKind kind) {
if (UseCRC32Intrinsics) {
address entry = __ pc();
// rbx,: Method*
// r13: senderSP must preserved for slow path, set SP to it on fast path
Label slow_path;
// If we need a safepoint check, generate full interpreter entry.
ExternalAddress state(SafepointSynchronize::address_of_state());
__ cmp32(ExternalAddress(SafepointSynchronize::address_of_state()),
SafepointSynchronize::_not_synchronized);
__ jcc(Assembler::notEqual, slow_path);
// We don't generate local frame and don't align stack because
// we call stub code and there is no safepoint on this path.
// Load parameters
const Register crc = c_rarg0; // crc
const Register buf = c_rarg1; // source java byte array address
const Register len = c_rarg2; // length
const Register off = len; // offset (never overlaps with 'len')
// Arguments are reversed on java expression stack
// Calculate address of start element
if (kind == Interpreter::java_util_zip_CRC32_updateByteBuffer) {
__ movptr(buf, Address(rsp, 3*wordSize)); // long buf
__ movl2ptr(off, Address(rsp, 2*wordSize)); // offset
__ addq(buf, off); // + offset
__ movl(crc, Address(rsp, 5*wordSize)); // Initial CRC
} else {
__ movptr(buf, Address(rsp, 3*wordSize)); // byte[] array
__ addptr(buf, arrayOopDesc::base_offset_in_bytes(T_BYTE)); // + header size
__ movl2ptr(off, Address(rsp, 2*wordSize)); // offset
__ addq(buf, off); // + offset
__ movl(crc, Address(rsp, 4*wordSize)); // Initial CRC
}
// Can now load 'len' since we're finished with 'off'
__ movl(len, Address(rsp, wordSize)); // Length
__ super_call_VM_leaf(CAST_FROM_FN_PTR(address, StubRoutines::updateBytesCRC32()), crc, buf, len);
// result in rax
// _areturn
__ pop(rdi); // get return address
__ mov(rsp, r13); // set sp to sender sp
__ jmp(rdi);
// generate a vanilla native entry as the slow path
__ bind(slow_path);
__ jump_to_entry(Interpreter::entry_for_kind(Interpreter::native));
return entry;
}
return NULL;
}
void taskUpdateBattery(void)
{
#ifdef USE_ADC
static uint32_t vbatLastServiced = 0;
if (feature(FEATURE_VBAT)) {
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
updateBattery();
}
}
#endif
static uint32_t ibatLastServiced = 0;
if (feature(FEATURE_CURRENT_METER)) {
int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime;
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
}
}
}
void batteryUpdateCurrentMeter(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
if (batteryCellCount == 0) {
currentMeterReset(¤tMeter);
return;
}
static uint32_t ibatLastServiced = 0;
const int32_t lastUpdateAt = cmp32(currentTimeUs, ibatLastServiced);
ibatLastServiced = currentTimeUs;
switch (batteryConfig()->currentMeterSource) {
case CURRENT_METER_ADC:
currentMeterADCRefresh(lastUpdateAt);
currentMeterADCRead(¤tMeter);
break;
case CURRENT_METER_VIRTUAL: {
#ifdef USE_VIRTUAL_CURRENT_METER
throttleStatus_e throttleStatus = calculateThrottleStatus();
bool throttleLowAndMotorStop = (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP));
int32_t throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000;
currentMeterVirtualRefresh(lastUpdateAt, ARMING_FLAG(ARMED), throttleLowAndMotorStop, throttleOffset);
currentMeterVirtualRead(¤tMeter);
#endif
break;
}
case CURRENT_METER_ESC:
#ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR)) {
currentMeterESCRefresh(lastUpdateAt);
currentMeterESCReadCombined(¤tMeter);
}
#endif
break;
case CURRENT_METER_MSP:
#ifdef USE_MSP_CURRENT_METER
currentMeterMSPRefresh(currentTimeUs);
currentMeterMSPRead(¤tMeter);
#endif
break;
default:
case CURRENT_METER_NONE:
currentMeterReset(¤tMeter);
break;
}
}
void taskUpdateBattery(void)
{
static uint32_t vbatLastServiced = 0;
static uint32_t ibatLastServiced = 0;
if (feature(FEATURE_VBAT)) {
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
updateBattery();
}
}
if (feature(FEATURE_CURRENT_METER)) {
int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime;
throttleStatus_e throttleStatus = calculateThrottleStatus(rxConfig(), rcControlsConfig()->deadband3d_throttle);
updateCurrentMeter(ibatTimeSinceLastServiced, throttleStatus);
}
}
}
void ledStripUpdate(uint32_t currentTime)
{
if (!(ledStripInitialised && isWS2811LedStripReady())) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXLEDLOW) && !(currentLedStripConfig->ledstrip_visual_beeper && isBeeperOn())) {
if (ledStripEnabled) {
ledStripDisable();
ledStripEnabled = false;
}
return;
}
ledStripEnabled = true;
const uint32_t now = currentTime;
// test all led timers, setting corresponding bits
uint32_t timActive = 0;
for (timId_e timId = 0; timId < timTimerCount; timId++) {
// sanitize timer value, so that it can be safely incremented. Handles inital timerVal value.
// max delay is limited to 5s
int32_t delta = cmp32(now, timerVal[timId]);
if (delta < 0 && delta > -LED_STRIP_MS(5000))
continue; // not ready yet
timActive |= 1 << timId;
if (delta >= LED_STRIP_MS(100) || delta < 0) {
timerVal[timId] = now;
}
}
if (!timActive)
return; // no change this update, keep old state
// apply all layers; triggered timed functions has to update timers
scaledThrottle = ARMING_FLAG(ARMED) ? scaleRange(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX, 10, 100) : 10;
scaledAux = scaleRange(rcData[currentLedStripConfig->ledstrip_aux_channel], PWM_RANGE_MIN, PWM_RANGE_MAX, 0, HSV_HUE_MAX + 1);
applyLedFixedLayers();
for (timId_e timId = 0; timId < ARRAYLEN(layerTable); timId++) {
uint32_t *timer = &timerVal[timId];
bool updateNow = timActive & (1 << timId);
(*layerTable[timId])(updateNow, timer);
}
ws2811UpdateStrip();
}
/**
* Method entry for static native methods:
* int java.util.zip.CRC32.update(int crc, int b)
*/
address TemplateInterpreterGenerator::generate_CRC32_update_entry() {
if (UseCRC32Intrinsics) {
address entry = __ pc();
// rbx,: Method*
// r13: senderSP must preserved for slow path, set SP to it on fast path
// c_rarg0: scratch (rdi on non-Win64, rcx on Win64)
// c_rarg1: scratch (rsi on non-Win64, rdx on Win64)
Label slow_path;
// If we need a safepoint check, generate full interpreter entry.
ExternalAddress state(SafepointSynchronize::address_of_state());
__ cmp32(ExternalAddress(SafepointSynchronize::address_of_state()),
SafepointSynchronize::_not_synchronized);
__ jcc(Assembler::notEqual, slow_path);
// We don't generate local frame and don't align stack because
// we call stub code and there is no safepoint on this path.
// Load parameters
const Register crc = rax; // crc
const Register val = c_rarg0; // source java byte value
const Register tbl = c_rarg1; // scratch
// Arguments are reversed on java expression stack
__ movl(val, Address(rsp, wordSize)); // byte value
__ movl(crc, Address(rsp, 2*wordSize)); // Initial CRC
__ lea(tbl, ExternalAddress(StubRoutines::crc_table_addr()));
__ notl(crc); // ~crc
__ update_byte_crc32(crc, val, tbl);
__ notl(crc); // ~crc
// result in rax
// _areturn
__ pop(rdi); // get return address
__ mov(rsp, r13); // set sp to sender sp
__ jmp(rdi);
// generate a vanilla native entry as the slow path
__ bind(slow_path);
__ jump_to_entry(Interpreter::entry_for_kind(Interpreter::native));
return entry;
}
return NULL;
}
bool osdSlaveCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
{
UNUSED(currentTimeUs);
UNUSED(currentDeltaTimeUs);
if (!stalled && (cmp32(currentTimeUs, timeoutAt) > 0)) {
stalled = true;
displayWrite(osdDisplayPort, 8, 12, "WAITING FOR FC");
displayResync(osdDisplayPort);
}
if (!receivingScreen && !displayIsSynced(osdDisplayPort)) {
// queue a screen draw to ensure any remaining characters not written to the screen yet
// remember that displayDrawScreen() may return WITHOUT having fully updated the screen.
displayDrawScreenQueued = true;
}
return receivingScreen || displayDrawScreenQueued;
}
void mspClientProcess(void)
{
static uint8_t index = 0;
bool busy = mspPorts[1].commandSenderFn != NULL;
if (busy) {
return;
}
commandToSend = commandsToSend[index];
mspPorts[1].commandSenderFn = mspRequestFCSimpleCommandSender;
index++;
if (index >= ARRAYLEN(commandsToSend)) {
index = 0;
}
//
// handle timeout of received data.
//
uint32_t now = micros();
mspClientStatus.timeoutOccured = (cmp32(now, mspClientStatus.lastReplyAt) >= MSP_CLIENT_TIMEOUT_INTERVAL);
}
address JNI_FastGetField::generate_fast_get_float_field0(BasicType type) {
const char *name = NULL;
switch (type) {
case T_FLOAT: name = "jni_fast_GetFloatField"; break;
case T_DOUBLE: name = "jni_fast_GetDoubleField"; break;
default: ShouldNotReachHere();
}
ResourceMark rm;
BufferBlob* blob = BufferBlob::create(name, BUFFER_SIZE);
CodeBuffer cbuf(blob);
MacroAssembler* masm = new MacroAssembler(&cbuf);
address fast_entry = __ pc();
Label slow;
ExternalAddress counter(SafepointSynchronize::safepoint_counter_addr());
__ mov32 (rcounter, counter);
__ mov (robj, c_rarg1);
__ testb (rcounter, 1);
__ jcc (Assembler::notZero, slow);
if (os::is_MP()) {
__ xorptr(robj, rcounter);
__ xorptr(robj, rcounter); // obj, since
// robj ^ rcounter ^ rcounter == robj
// robj is data dependent on rcounter.
}
__ clear_jweak_tag(robj);
__ movptr(robj, Address(robj, 0)); // *obj
__ mov (roffset, c_rarg2);
__ shrptr(roffset, 2); // offset
assert(count < LIST_CAPACITY, "LIST_CAPACITY too small");
speculative_load_pclist[count] = __ pc();
switch (type) {
case T_FLOAT: __ movflt (xmm0, Address(robj, roffset, Address::times_1)); break;
case T_DOUBLE: __ movdbl (xmm0, Address(robj, roffset, Address::times_1)); break;
default: ShouldNotReachHere();
}
if (os::is_MP()) {
__ lea(rcounter_addr, counter);
__ movdq (rax, xmm0);
// counter address is data dependent on xmm0.
__ xorptr(rcounter_addr, rax);
__ xorptr(rcounter_addr, rax);
__ cmpl (rcounter, Address(rcounter_addr, 0));
} else {
__ cmp32 (rcounter, counter);
}
__ jcc (Assembler::notEqual, slow);
__ ret (0);
slowcase_entry_pclist[count++] = __ pc();
__ bind (slow);
address slow_case_addr = NULL;
switch (type) {
case T_FLOAT: slow_case_addr = jni_GetFloatField_addr(); break;
case T_DOUBLE: slow_case_addr = jni_GetDoubleField_addr();
}
// tail call
__ jump (ExternalAddress(slow_case_addr));
__ flush ();
return fast_entry;
}
address JNI_FastGetField::generate_fast_get_int_field0(BasicType type) {
const char *name;
switch (type) {
case T_BOOLEAN: name = "jni_fast_GetBooleanField"; break;
case T_BYTE: name = "jni_fast_GetByteField"; break;
case T_CHAR: name = "jni_fast_GetCharField"; break;
case T_SHORT: name = "jni_fast_GetShortField"; break;
case T_INT: name = "jni_fast_GetIntField"; break;
default: ShouldNotReachHere();
}
ResourceMark rm;
BufferBlob* b = BufferBlob::create(name, BUFFER_SIZE*wordSize);
address fast_entry = b->instructions_begin();
CodeBuffer cbuf(fast_entry, b->instructions_size());
MacroAssembler* masm = new MacroAssembler(&cbuf);
Label slow;
// stack layout: offset from rsp (in words):
// return pc 0
// jni env 1
// obj 2
// jfieldID 3
ExternalAddress counter(SafepointSynchronize::safepoint_counter_addr());
__ mov32 (rcx, counter);
__ testb (rcx, 1);
__ jcc (Assembler::notZero, slow);
if (os::is_MP()) {
__ mov(rax, rcx);
__ andptr(rax, 1); // rax, must end up 0
__ movptr(rdx, Address(rsp, rax, Address::times_1, 2*wordSize));
// obj, notice rax, is 0.
// rdx is data dependent on rcx.
} else {
__ movptr (rdx, Address(rsp, 2*wordSize)); // obj
}
__ movptr(rax, Address(rsp, 3*wordSize)); // jfieldID
__ movptr(rdx, Address(rdx, 0)); // *obj
__ shrptr (rax, 2); // offset
assert(count < LIST_CAPACITY, "LIST_CAPACITY too small");
speculative_load_pclist[count] = __ pc();
switch (type) {
case T_BOOLEAN: __ movzbl (rax, Address(rdx, rax, Address::times_1)); break;
case T_BYTE: __ movsbl (rax, Address(rdx, rax, Address::times_1)); break;
case T_CHAR: __ movzwl (rax, Address(rdx, rax, Address::times_1)); break;
case T_SHORT: __ movswl (rax, Address(rdx, rax, Address::times_1)); break;
case T_INT: __ movl (rax, Address(rdx, rax, Address::times_1)); break;
default: ShouldNotReachHere();
}
Address ca1;
if (os::is_MP()) {
__ lea(rdx, counter);
__ xorptr(rdx, rax);
__ xorptr(rdx, rax);
__ cmp32(rcx, Address(rdx, 0));
// ca1 is the same as ca because
// rax, ^ counter_addr ^ rax, = address
// ca1 is data dependent on rax,.
} else {
__ cmp32(rcx, counter);
}
__ jcc (Assembler::notEqual, slow);
#ifndef _WINDOWS
__ ret (0);
#else
// __stdcall calling convention
__ ret (3*wordSize);
#endif
slowcase_entry_pclist[count++] = __ pc();
__ bind (slow);
address slow_case_addr;
switch (type) {
case T_BOOLEAN: slow_case_addr = jni_GetBooleanField_addr(); break;
case T_BYTE: slow_case_addr = jni_GetByteField_addr(); break;
case T_CHAR: slow_case_addr = jni_GetCharField_addr(); break;
case T_SHORT: slow_case_addr = jni_GetShortField_addr(); break;
case T_INT: slow_case_addr = jni_GetIntField_addr();
}
// tail call
__ jump (ExternalAddress(slow_case_addr));
__ flush ();
#ifndef _WINDOWS
return fast_entry;
#else
switch (type) {
case T_BOOLEAN: jni_fast_GetBooleanField_fp = (GetBooleanField_t)fast_entry; break;
case T_BYTE: jni_fast_GetByteField_fp = (GetByteField_t)fast_entry; break;
case T_CHAR: jni_fast_GetCharField_fp = (GetCharField_t)fast_entry; break;
case T_SHORT: jni_fast_GetShortField_fp = (GetShortField_t)fast_entry; break;
case T_INT: jni_fast_GetIntField_fp = (GetIntField_t)fast_entry;
}
return os::win32::fast_jni_accessor_wrapper(type);
#endif
}
address JNI_FastGetField::generate_fast_get_float_field0(BasicType type) {
const char *name;
switch (type) {
case T_FLOAT: name = "jni_fast_GetFloatField"; break;
case T_DOUBLE: name = "jni_fast_GetDoubleField"; break;
default: ShouldNotReachHere();
}
ResourceMark rm;
BufferBlob* b = BufferBlob::create(name, BUFFER_SIZE*wordSize);
address fast_entry = b->instructions_begin();
CodeBuffer cbuf(fast_entry, b->instructions_size());
MacroAssembler* masm = new MacroAssembler(&cbuf);
Label slow_with_pop, slow;
// stack layout: offset from rsp (in words):
// return pc 0
// jni env 1
// obj 2
// jfieldID 3
ExternalAddress counter(SafepointSynchronize::safepoint_counter_addr());
__ mov32 (rcx, counter);
__ testb (rcx, 1);
__ jcc (Assembler::notZero, slow);
if (os::is_MP()) {
__ mov(rax, rcx);
__ andptr(rax, 1); // rax, must end up 0
__ movptr(rdx, Address(rsp, rax, Address::times_1, 2*wordSize));
// obj, notice rax, is 0.
// rdx is data dependent on rcx.
} else {
__ movptr(rdx, Address(rsp, 2*wordSize)); // obj
}
__ movptr(rax, Address(rsp, 3*wordSize)); // jfieldID
__ movptr(rdx, Address(rdx, 0)); // *obj
__ shrptr(rax, 2); // offset
assert(count < LIST_CAPACITY, "LIST_CAPACITY too small");
speculative_load_pclist[count] = __ pc();
switch (type) {
#ifndef _LP64
case T_FLOAT: __ fld_s (Address(rdx, rax, Address::times_1)); break;
case T_DOUBLE: __ fld_d (Address(rdx, rax, Address::times_1)); break;
#else
case T_FLOAT: __ movflt (xmm0, Address(robj, roffset, Address::times_1)); break;
case T_DOUBLE: __ movdbl (xmm0, Address(robj, roffset, Address::times_1)); break;
#endif // _LP64
default: ShouldNotReachHere();
}
Address ca1;
if (os::is_MP()) {
__ fst_s (Address(rsp, -4));
__ lea(rdx, counter);
__ movl (rax, Address(rsp, -4));
// garbage hi-order bits on 64bit are harmless.
__ xorptr(rdx, rax);
__ xorptr(rdx, rax);
__ cmp32(rcx, Address(rdx, 0));
// rax, ^ counter_addr ^ rax, = address
// ca1 is data dependent on the field
// access.
} else {
__ cmp32(rcx, counter);
}
__ jcc (Assembler::notEqual, slow_with_pop);
#ifndef _WINDOWS
__ ret (0);
#else
// __stdcall calling convention
__ ret (3*wordSize);
#endif
__ bind (slow_with_pop);
// invalid load. pop FPU stack.
__ fstp_d (0);
slowcase_entry_pclist[count++] = __ pc();
__ bind (slow);
address slow_case_addr;
switch (type) {
case T_FLOAT: slow_case_addr = jni_GetFloatField_addr(); break;
case T_DOUBLE: slow_case_addr = jni_GetDoubleField_addr(); break;
default: ShouldNotReachHere();
}
// tail call
__ jump (ExternalAddress(slow_case_addr));
__ flush ();
#ifndef _WINDOWS
return fast_entry;
#else
switch (type) {
case T_FLOAT: jni_fast_GetFloatField_fp = (GetFloatField_t)fast_entry; break;
case T_DOUBLE: jni_fast_GetDoubleField_fp = (GetDoubleField_t)fast_entry;
}
return os::win32::fast_jni_accessor_wrapper(type);
#endif
}
address JNI_FastGetField::generate_fast_get_long_field() {
const char *name = "jni_fast_GetLongField";
ResourceMark rm;
BufferBlob* b = BufferBlob::create(name, BUFFER_SIZE*wordSize);
address fast_entry = b->instructions_begin();
CodeBuffer cbuf(fast_entry, b->instructions_size());
MacroAssembler* masm = new MacroAssembler(&cbuf);
Label slow;
// stack layout: offset from rsp (in words):
// old rsi 0
// return pc 1
// jni env 2
// obj 3
// jfieldID 4
ExternalAddress counter(SafepointSynchronize::safepoint_counter_addr());
__ push (rsi);
__ mov32 (rcx, counter);
__ testb (rcx, 1);
__ jcc (Assembler::notZero, slow);
if (os::is_MP()) {
__ mov(rax, rcx);
__ andptr(rax, 1); // rax, must end up 0
__ movptr(rdx, Address(rsp, rax, Address::times_1, 3*wordSize));
// obj, notice rax, is 0.
// rdx is data dependent on rcx.
} else {
__ movptr(rdx, Address(rsp, 3*wordSize)); // obj
}
__ movptr(rsi, Address(rsp, 4*wordSize)); // jfieldID
__ movptr(rdx, Address(rdx, 0)); // *obj
__ shrptr(rsi, 2); // offset
assert(count < LIST_CAPACITY-1, "LIST_CAPACITY too small");
speculative_load_pclist[count++] = __ pc();
__ movptr(rax, Address(rdx, rsi, Address::times_1));
#ifndef _LP64
speculative_load_pclist[count] = __ pc();
__ movl(rdx, Address(rdx, rsi, Address::times_1, 4));
#endif // _LP64
if (os::is_MP()) {
__ lea(rsi, counter);
__ xorptr(rsi, rdx);
__ xorptr(rsi, rax);
__ xorptr(rsi, rdx);
__ xorptr(rsi, rax);
__ cmp32(rcx, Address(rsi, 0));
// ca1 is the same as ca because
// rax, ^ rdx ^ counter_addr ^ rax, ^ rdx = address
// ca1 is data dependent on both rax, and rdx.
} else {
__ cmp32(rcx, counter);
}
__ jcc (Assembler::notEqual, slow);
__ pop (rsi);
#ifndef _WINDOWS
__ ret (0);
#else
// __stdcall calling convention
__ ret (3*wordSize);
#endif
slowcase_entry_pclist[count-1] = __ pc();
slowcase_entry_pclist[count++] = __ pc();
__ bind (slow);
__ pop (rsi);
address slow_case_addr = jni_GetLongField_addr();;
// tail call
__ jump (ExternalAddress(slow_case_addr));
__ flush ();
#ifndef _WINDOWS
return fast_entry;
#else
jni_fast_GetLongField_fp = (GetLongField_t)fast_entry;
return os::win32::fast_jni_accessor_wrapper(T_LONG);
#endif
}
void annexCode(void)
{
int32_t tmp, tmp2;
int32_t axis, prop1 = 0, prop2;
static uint32_t vbatLastServiced = 0;
static uint32_t ibatLastServiced = 0;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
prop2 = 100;
} else {
if (rcData[THROTTLE] < 2000) {
prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint);
} else {
prop2 = 100 - currentControlRateProfile->dynThrPID;
}
}
for (axis = 0; axis < 3; axis++) {
tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500);
if (axis == ROLL || axis == PITCH) {
if (currentProfile->rcControlsConfig.deadband) {
if (tmp > currentProfile->rcControlsConfig.deadband) {
tmp -= currentProfile->rcControlsConfig.deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500;
prop1 = (uint16_t)prop1 * prop2 / 100;
} else if (axis == YAW) {
if (currentProfile->rcControlsConfig.yaw_deadband) {
if (tmp > currentProfile->rcControlsConfig.yaw_deadband) {
tmp -= currentProfile->rcControlsConfig.yaw_deadband;
} else {
tmp = 0;
}
}
tmp2 = tmp / 100;
rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction;
prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500;
}
// FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling.
dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100;
dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100;
dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100;
// non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids
if (axis == YAW) {
PIDweight[axis] = 100;
}
else {
PIDweight[axis] = prop2;
}
if (rcData[axis] < masterConfig.rxConfig.midrc)
rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX);
tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if (FLIGHT_MODE(HEADFREE_MODE)) {
float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
float cosDiff = cos_approx(radDiff);
float sinDiff = sin_approx(radDiff);
int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}
if (feature(FEATURE_VBAT)) {
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
vbatLastServiced = currentTime;
updateBattery();
}
}
if (feature(FEATURE_CURRENT_METER)) {
int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced);
if (ibatTimeSinceLastServiced >= IBATINTERVAL) {
ibatLastServiced = currentTime;
updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
}
}
beeperUpdate(); //call periodic beeper handler
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
if (IS_RC_MODE_ACTIVE(BOXARM) == 0) {
ENABLE_ARMING_FLAG(OK_TO_ARM);
}
if (!STATE(SMALL_ANGLE)) {
DISABLE_ARMING_FLAG(OK_TO_ARM);
}
if (isCalibrating()) {
warningLedFlash();
DISABLE_ARMING_FLAG(OK_TO_ARM);
} else {
if (ARMING_FLAG(OK_TO_ARM)) {
warningLedDisable();
} else {
warningLedFlash();
}
}
warningLedUpdate();
}
#ifdef TELEMETRY
telemetryCheckState();
#endif
handleSerial();
#ifdef GPS
if (sensors(SENSOR_GPS)) {
updateGpsIndicator(currentTime);
}
#endif
// Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities.
if (gyro.temperature)
gyro.temperature(&telemTemperature1);
}
void processRcAdjustments(controlRateConfig_t *controlRateConfig)
{
const uint32_t now = millis();
int newValue = -1;
const bool canUseRxData = rxIsReceivingSignal();
for (int adjustmentIndex = 0; adjustmentIndex < MAX_SIMULTANEOUS_ADJUSTMENT_COUNT; adjustmentIndex++) {
adjustmentState_t *adjustmentState = &adjustmentStates[adjustmentIndex];
if (!adjustmentState->config) {
continue;
}
const uint8_t adjustmentFunction = adjustmentState->config->adjustmentFunction;
if (adjustmentFunction == ADJUSTMENT_NONE) {
continue;
}
if (cmp32(now, adjustmentState->timeoutAt) >= 0) {
adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
#if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
adjustmentRangeValue = -1;
#endif
}
if (!canUseRxData) {
continue;
}
const uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentState->auxChannelIndex;
if (adjustmentState->config->mode == ADJUSTMENT_MODE_STEP) {
int delta;
if (rcData[channelIndex] > rxConfig()->midrc + 200) {
delta = adjustmentState->config->data.step;
} else if (rcData[channelIndex] < rxConfig()->midrc - 200) {
delta = -adjustmentState->config->data.step;
} else {
// returning the switch to the middle immediately resets the ready state
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentIndex);
adjustmentState->timeoutAt = now + RESET_FREQUENCY_2HZ;
continue;
}
if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentIndex)) {
continue;
}
newValue = applyStepAdjustment(controlRateConfig, adjustmentFunction, delta);
pidInitConfig(pidProfile);
} else if (adjustmentState->config->mode == ADJUSTMENT_MODE_SELECT) {
const uint16_t rangeWidth = ((2100 - 900) / adjustmentState->config->data.switchPositions);
const uint8_t position = (constrain(rcData[channelIndex], 900, 2100 - 1) - 900) / rangeWidth;
newValue = applySelectAdjustment(adjustmentFunction, position);
}
#if defined(USE_OSD) && defined(USE_OSD_ADJUSTMENTS)
if (newValue != -1 && adjustmentState->config->adjustmentFunction != ADJUSTMENT_RATE_PROFILE) { // Rate profile already has an OSD element
adjustmentRangeName = &adjustmentLabels[adjustmentFunction - 1][0];
adjustmentRangeValue = newValue;
}
#else
UNUSED(newValue);
#endif
MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentIndex);
}
}
void osdUpdate(void)
{
TIME_SECTION_BEGIN(0);
uint32_t now = micros();
static bool armed = false;
static uint32_t armedAt = 0;
bool armedNow = fcStatus.fcState & (1 << FC_STATE_ARM);
if (armed != armedNow) {
if (armedNow) {
armedAt = now;
}
armed = armedNow;
}
if (armed) {
if (now > armedAt) {
fcStatus.armedDuration = (now - armedAt) / 1000;
}
}
osdClearScreen();
// test all timers, setting corresponding bits
uint32_t timActive = 0;
for(timId_e timId = 0; timId < timTimerCount; timId++) {
if(cmp32(now, timerState[timId].val) < 0)
continue; // not ready yet
timActive |= 1 << timId;
// sanitize timer value, so that it can be safely incremented. Handles inital timerVal value.
// max delay is limited to 5s
if(cmp32(now, timerState[timId].val) >= OSD_MS(100) || cmp32(now, timerState[timId].val) < OSD_HZ(5000) ) {
timerState[timId].val = now;
}
}
// update all timers
for(unsigned i = 0; i < ARRAYLEN(timerTable); i++) {
int timId = timerTable[i].timId;
bool updateNow = timActive & (1 << timId);
if (!updateNow) {
continue;
}
timerState[timId].toggle = !timerState[timId].toggle;
timerState[timId].counter++;
timerState[timId].val += timerTable[i].delay;
}
bool showNowOrFlashWhenFCTimeoutOccured = !mspClientStatus.timeoutOccured || (mspClientStatus.timeoutOccured && timerState[tim10Hz].toggle);
osdSetElementFlashOnDisconnectState(showNowOrFlashWhenFCTimeoutOccured);
for (uint8_t elementIndex = 0; elementIndex < MAX_OSD_ELEMENT_COUNT; elementIndex++) {
element_t *element = &osdElementConfig()->elements[elementIndex];
osdDrawTextElement(element);
}
//
// flash the logo for a few seconds
// then leave it on for a few more before turning it off
//
static bool splashDone = false;
if (!splashDone) {
if (
(timerState[tim1Hz].counter > 3 && timerState[tim1Hz].counter < 6) ||
(timerState[tim1Hz].counter <= 3 && timerState[tim10Hz].toggle)
) {
osdDisplaySplash();
}
if (timerState[tim1Hz].counter >= 10) {
splashDone = true;
}
}
if (timerState[tim2Hz].toggle && splashDone) {
if (!osdIsCameraConnected()) {
osdPrintAt(14, 4, "NO");
osdPrintAt(12, 5, "CAMERA");
}
}
TIME_SECTION_END(0);
osdHardwareUpdate();
}
STATIC_UNIT_TESTED void osdRefresh(timeUs_t currentTimeUs)
{
static timeUs_t lastTimeUs = 0;
static bool osdStatsEnabled = false;
static bool osdStatsVisible = false;
static timeUs_t osdStatsRefreshTimeUs;
static uint16_t endBatteryVoltage;
// detect arm/disarm
if (armState != ARMING_FLAG(ARMED)) {
if (ARMING_FLAG(ARMED)) {
osdStatsEnabled = false;
osdStatsVisible = false;
osdResetStats();
osdShowArmed();
resumeRefreshAt = currentTimeUs + (REFRESH_1S / 2);
} else if (isSomeStatEnabled()
&& (!(getArmingDisableFlags() & ARMING_DISABLED_RUNAWAY_TAKEOFF)
|| !VISIBLE(osdConfig()->item_pos[OSD_WARNINGS]))) { // suppress stats if runaway takeoff triggered disarm and WARNINGS element is visible
osdStatsEnabled = true;
resumeRefreshAt = currentTimeUs + (60 * REFRESH_1S);
endBatteryVoltage = getBatteryVoltage();
}
armState = ARMING_FLAG(ARMED);
}
if (ARMING_FLAG(ARMED)) {
osdUpdateStats();
timeUs_t deltaT = currentTimeUs - lastTimeUs;
flyTime += deltaT;
stats.armed_time += deltaT;
} else if (osdStatsEnabled) { // handle showing/hiding stats based on OSD disable switch position
if (displayIsGrabbed(osdDisplayPort)) {
osdStatsEnabled = false;
resumeRefreshAt = 0;
stats.armed_time = 0;
} else {
if (IS_RC_MODE_ACTIVE(BOXOSD) && osdStatsVisible) {
osdStatsVisible = false;
displayClearScreen(osdDisplayPort);
} else if (!IS_RC_MODE_ACTIVE(BOXOSD)) {
if (!osdStatsVisible) {
osdStatsVisible = true;
osdStatsRefreshTimeUs = 0;
}
if (currentTimeUs >= osdStatsRefreshTimeUs) {
osdStatsRefreshTimeUs = currentTimeUs + REFRESH_1S;
osdShowStats(endBatteryVoltage);
}
}
}
}
lastTimeUs = currentTimeUs;
if (resumeRefreshAt) {
if (cmp32(currentTimeUs, resumeRefreshAt) < 0) {
// in timeout period, check sticks for activity to resume display.
if (IS_HI(THROTTLE) || IS_HI(PITCH)) {
resumeRefreshAt = currentTimeUs;
}
displayHeartbeat(osdDisplayPort);
return;
} else {
displayClearScreen(osdDisplayPort);
resumeRefreshAt = 0;
osdStatsEnabled = false;
stats.armed_time = 0;
}
}
blinkState = (currentTimeUs / 200000) % 2;
#ifdef USE_ESC_SENSOR
if (feature(FEATURE_ESC_SENSOR)) {
escDataCombined = getEscSensorData(ESC_SENSOR_COMBINED);
}
#endif
#ifdef USE_CMS
if (!displayIsGrabbed(osdDisplayPort)) {
osdUpdateAlarms();
osdDrawElements();
displayHeartbeat(osdDisplayPort);
#ifdef OSD_CALLS_CMS
} else {
cmsUpdate(currentTimeUs);
#endif
}
#endif
lastArmState = ARMING_FLAG(ARMED);
}
address JNI_FastGetField::generate_fast_get_int_field0(BasicType type) {
const char *name;
switch (type) {
case T_BOOLEAN: name = "jni_fast_GetBooleanField"; break;
case T_BYTE: name = "jni_fast_GetByteField"; break;
case T_CHAR: name = "jni_fast_GetCharField"; break;
case T_SHORT: name = "jni_fast_GetShortField"; break;
case T_INT: name = "jni_fast_GetIntField"; break;
case T_LONG: name = "jni_fast_GetLongField"; break;
default: ShouldNotReachHere();
}
ResourceMark rm;
BufferBlob* b = BufferBlob::create(name, BUFFER_SIZE);
address fast_entry = b->instructions_begin();
CodeBuffer cbuf(fast_entry, b->instructions_size());
MacroAssembler* masm = new MacroAssembler(&cbuf);
Label slow;
ExternalAddress counter(SafepointSynchronize::safepoint_counter_addr());
__ mov32 (rcounter, counter);
__ mov (robj, c_rarg1);
__ testb (rcounter, 1);
__ jcc (Assembler::notZero, slow);
if (os::is_MP()) {
__ xorptr(robj, rcounter);
__ xorptr(robj, rcounter); // obj, since
// robj ^ rcounter ^ rcounter == robj
// robj is data dependent on rcounter.
}
__ movptr(robj, Address(robj, 0)); // *obj
__ mov (roffset, c_rarg2);
__ shrptr(roffset, 2); // offset
assert(count < LIST_CAPACITY, "LIST_CAPACITY too small");
speculative_load_pclist[count] = __ pc();
switch (type) {
case T_BOOLEAN: __ movzbl (rax, Address(robj, roffset, Address::times_1)); break;
case T_BYTE: __ movsbl (rax, Address(robj, roffset, Address::times_1)); break;
case T_CHAR: __ movzwl (rax, Address(robj, roffset, Address::times_1)); break;
case T_SHORT: __ movswl (rax, Address(robj, roffset, Address::times_1)); break;
case T_INT: __ movl (rax, Address(robj, roffset, Address::times_1)); break;
case T_LONG: __ movq (rax, Address(robj, roffset, Address::times_1)); break;
default: ShouldNotReachHere();
}
if (os::is_MP()) {
__ lea(rcounter_addr, counter);
// ca is data dependent on rax.
__ xorptr(rcounter_addr, rax);
__ xorptr(rcounter_addr, rax);
__ cmpl (rcounter, Address(rcounter_addr, 0));
} else {
__ cmp32 (rcounter, counter);
}
__ jcc (Assembler::notEqual, slow);
__ ret (0);
slowcase_entry_pclist[count++] = __ pc();
__ bind (slow);
address slow_case_addr;
switch (type) {
case T_BOOLEAN: slow_case_addr = jni_GetBooleanField_addr(); break;
case T_BYTE: slow_case_addr = jni_GetByteField_addr(); break;
case T_CHAR: slow_case_addr = jni_GetCharField_addr(); break;
case T_SHORT: slow_case_addr = jni_GetShortField_addr(); break;
case T_INT: slow_case_addr = jni_GetIntField_addr(); break;
case T_LONG: slow_case_addr = jni_GetLongField_addr();
}
// tail call
__ jump (ExternalAddress(slow_case_addr));
__ flush ();
return fast_entry;
}