void can_write_vstats(unsigned int StringNo, unsigned int* vcell, unsigned int* temperature, unsigned int isense)
{
unsigned int i, a;
//Prepare a CAN message for these cell voltages
if(StringNo == BATT_S1) {can_push_ptr->address = BMS_S1_CAN_BASE;}
else {can_push_ptr->address = BMS_S2_CAN_BASE;}
can_push_ptr->address += BMS_STAT;
// Sum up all cell voltages in stack
a = 0;
for ( i = 0; i < 30; i++ )
{
a += ( vcell[i] / 10 );
}
can_push_ptr->data.data_u16[0] = a;
// Average temperature
a = temperature[1] + temperature[4] + temperature[7];
a /= 3;
can_push_ptr->data.data_u16[1] = a;
// Current measurement
can_push_ptr->data.data_u16[2] = isense;
can_push_ptr->status = 6;
can_push();
can_transmit();
}
/* push 32 bit operand size */
void
bx_cpu_c::push_32(Bit32u value32)
{
/* must use StackAddrSize, and either ESP or SP accordingly */
if (bx_cpu. sregs[BX_SEG_REG_SS].cache.u.segment.d_b) { /* StackAddrSize = 32 */
/* 32bit stack size: pushes use SS:ESP */
if (protected_mode()) {
if (!can_push(&bx_cpu. sregs[BX_SEG_REG_SS].cache, ESP, 4)) {
BX_PANIC(("push_32(): push outside stack limits"));
/* #SS(0) */
}
}
else { /* real mode */
if ((ESP>=1) && (ESP<=3)) {
BX_PANIC(("push_32: ESP=%08x", (unsigned) ESP));
}
}
write_virtual_dword(BX_SEG_REG_SS, ESP-4, &value32);
ESP -= 4;
/* will return after error anyway */
return;
}
else { /* 16bit stack size: pushes use SS:SP */
if (protected_mode()) {
if (!can_push(&bx_cpu. sregs[BX_SEG_REG_SS].cache, SP, 4)) {
BX_PANIC(("push_32(): push outside stack limits"));
/* #SS(0) */
}
}
else { /* real mode */
if ((SP>=1) && (SP<=3)) {
BX_PANIC(("push_32: SP=%08x", (unsigned) SP));
}
}
write_virtual_dword(BX_SEG_REG_SS, (Bit16u) (SP-4), &value32);
SP -= 4;
/* will return after error anyway */
return;
}
}
void can_write_gear(unsigned int StringNo, unsigned int *gear_num)
{
//Prepare a CAN message for gear number
if(StringNo == SCU_GEAR_S1) {can_push_ptr->address = SCU_S1_CAN_BASE;}
can_push_ptr->address += SCU_GEAR;
can_push_ptr->data.data_u16[0] = gear_num[0];
can_push_ptr->current_gear = gear_num[0];
can_push_ptr->status = gear_num[0];
can_push();
can_transmit();
}
//Write set of 3 temperature readings from a single string to CAN bus
void can_write_temps(unsigned int StringNo, unsigned int* temp)
{
//Prepare a CAN message for these temperatures
if(StringNo == BATT_S1) {can_push_ptr->address = BMS_S1_CAN_BASE;}
else {can_push_ptr->address = BMS_S2_CAN_BASE;}
can_push_ptr->address += BMS_TEMP;
can_push_ptr->data.data_u16[0] = temp[1];
can_push_ptr->data.data_u16[1] = temp[4];
can_push_ptr->data.data_u16[2] = temp[7];
can_push_ptr->status = 6;
can_push();
can_transmit();
}
void
bx_cpu_c::push_16(Bit16u value16)
{
Bit32u temp_ESP;
#if BX_CPU_LEVEL >= 2
if (protected_mode()) {
#if BX_CPU_LEVEL >= 3
if (bx_cpu. sregs[BX_SEG_REG_SS].cache.u.segment.d_b)
temp_ESP = ESP;
else
#endif
temp_ESP = SP;
if (!can_push(&bx_cpu. sregs[BX_SEG_REG_SS].cache, temp_ESP, 2)) {
BX_PANIC(("push_16(): can't push on stack"));
exception(BX_SS_EXCEPTION, 0, 0);
return;
}
/* access within limits */
write_virtual_word(BX_SEG_REG_SS, temp_ESP - 2, &value16);
if (bx_cpu. sregs[BX_SEG_REG_SS].cache.u.segment.d_b)
ESP -= 2;
else
SP -= 2;
return;
}
else
#endif
{ /* real mode */
if (bx_cpu. sregs[BX_SEG_REG_SS].cache.u.segment.d_b) {
if (ESP == 1)
BX_PANIC(("CPU shutting down due to lack of stack space, ESP==1"));
ESP -= 2;
temp_ESP = ESP;
}
else {
if (SP == 1)
BX_PANIC(("CPU shutting down due to lack of stack space, SP==1"));
SP -= 2;
temp_ESP = SP;
}
write_virtual_word(BX_SEG_REG_SS, temp_ESP, &value16);
return;
}
}
bool MyDeque::push_front(char ch)
{
if (ready() == false)
return false;
bool ret = true;
// has space to push
if (can_push())
{
end_ = ((end_ + len_ - 1) % len_);
*(q_ + end_) = ch;
}
else
ret = false;
return ret;
}
static int addr_exp_op(struct addr_exp_state *s, char op)
{
if (op == '(') {
if (!s->last_operator || s->last_operator == ')')
goto syntax_error;
} else if (op == '-') {
if (s->last_operator && s->last_operator != ')')
op = 'N';
} else {
if (s->last_operator && s->last_operator != ')')
goto syntax_error;
}
if (op == ')') {
/* ) collapses the stack to the last matching ( */
while (s->op_stack_size &&
s->op_stack[s->op_stack_size - 1] != '(')
if (addr_exp_pop(s) < 0)
return -1;
if (!s->op_stack_size) {
fprintf(stderr, "parenthesis mismatch: )\n");
return -1;
}
s->op_stack_size--;
} else {
while (!can_push(s, op))
if (addr_exp_pop(s) < 0)
return -1;
if (s->op_stack_size + 1 > ARRAY_LEN(s->op_stack)) {
fprintf(stderr, "operator stack overflow: %c\n", op);
return -1;
}
s->op_stack[s->op_stack_size++] = op;
}
s->last_operator = op;
return 0;
syntax_error:
fprintf(stderr, "syntax error at operator %c\n", op);
return -1;
}
bool MyDeque::push_back(char ch)
{
if (ready() == false)
return false;
// copy ch to begin point
// ++begin
bool ret = true;
if (can_push())
{
*(q_ + begin_) = ch;
begin_ = ((begin_ + 1) % len_);
}
else
ret = false;
return ret;
}
void
bx_cpu_c::PUSHAD16(BxInstruction_t *i)
{
#if BX_CPU_LEVEL < 2
BX_PANIC(("PUSHAD: not supported on an 8086"));
#else
Bit32u temp_ESP;
Bit16u sp;
if (bx_cpu. sregs[BX_SEG_REG_SS].cache.u.segment.d_b)
temp_ESP = ESP;
else
temp_ESP = SP;
#if BX_CPU_LEVEL >= 2
if (protected_mode()) {
if ( !can_push(&bx_cpu. sregs[BX_SEG_REG_SS].cache, temp_ESP, 16) ) {
BX_PANIC(("PUSHA(): stack doesn't have enough room!"));
exception(BX_SS_EXCEPTION, 0, 0);
return;
}
}
else
#endif
{
if (temp_ESP < 16)
BX_PANIC(("pushad: eSP < 16"));
}
sp = SP;
/* ??? optimize this by using virtual write, all checks passed */
push_16(AX);
push_16(CX);
push_16(DX);
push_16(BX);
push_16(sp);
push_16(BP);
push_16(SI);
push_16(DI);
#endif
}
void
bx_cpu_c::CALL_Ed(BxInstruction_t *i)
{
Bit32u temp_ESP;
Bit32u op1_32;
#if BX_DEBUGGER
bx_cpu. show_flag |= Flag_call;
#endif
if (bx_cpu. sregs[BX_SEG_REG_SS].cache.u.segment.d_b)
temp_ESP = ESP;
else
temp_ESP = SP;
/* op1_32 is a register or memory reference */
if (i->mod == 0xc0) {
op1_32 = BX_READ_32BIT_REG(i->rm);
}
else {
read_virtual_dword(i->seg, i->rm_addr, &op1_32);
}
invalidate_prefetch_q();
if (protected_mode()) {
if (op1_32 > bx_cpu. sregs[BX_SEG_REG_CS].cache.u.segment.limit_scaled) {
BX_DEBUG(("call_ev: EIP out of CS limits! at %s:%d"));
exception(BX_GP_EXCEPTION, 0, 0);
}
if ( !can_push(&bx_cpu. sregs[BX_SEG_REG_SS].cache, temp_ESP, 4) ) {
BX_PANIC(("call_ev: can't push EIP"));
}
}
push_32(bx_cpu. eip);
bx_cpu. eip = op1_32;
BX_INSTR_UCNEAR_BRANCH(BX_INSTR_IS_CALL, bx_cpu. eip);
}
void can_write_vcell(unsigned int StringNo, unsigned int* vcell)
{
unsigned int i;
//Prepare a CAN message for these cell voltages
if(StringNo == BATT_S1) {can_push_ptr->address = BMS_S1_CAN_BASE;}
else {can_push_ptr->address = BMS_S2_CAN_BASE;}
can_push_ptr->address += BMS_CV;
for ( i = 0; i < 30; i += 4 )
{
can_push_ptr->data.data_u16[0] = vcell[i];
can_push_ptr->data.data_u16[1] = vcell[i+1];
if ( i+2 < 30 )
{
can_push_ptr->data.data_u16[2] = vcell[i+2];
can_push_ptr->data.data_u16[3] = vcell[i+3];
}
can_push_ptr->status = 8;
can_push();
can_transmit();
can_push_ptr->address++;
}
}
int pa_memblockq_push(pa_memblockq* bq, const pa_memchunk *uchunk) {
struct list_item *q, *n;
pa_memchunk chunk;
int64_t old;
pa_assert(bq);
pa_assert(uchunk);
pa_assert(uchunk->memblock);
pa_assert(uchunk->length > 0);
pa_assert(uchunk->index + uchunk->length <= pa_memblock_get_length(uchunk->memblock));
pa_assert(uchunk->length % bq->base == 0);
pa_assert(uchunk->index % bq->base == 0);
if (!can_push(bq, uchunk->length))
return -1;
old = bq->write_index;
chunk = *uchunk;
fix_current_write(bq);
q = bq->current_write;
/* First we advance the q pointer right of where we want to
* write to */
if (q) {
while (bq->write_index + (int64_t) chunk.length > q->index)
if (q->next)
q = q->next;
else
break;
}
if (!q)
q = bq->blocks_tail;
/* We go from back to front to look for the right place to add
* this new entry. Drop data we will overwrite on the way */
while (q) {
if (bq->write_index >= q->index + (int64_t) q->chunk.length)
/* We found the entry where we need to place the new entry immediately after */
break;
else if (bq->write_index + (int64_t) chunk.length <= q->index) {
/* This entry isn't touched at all, let's skip it */
q = q->prev;
} else if (bq->write_index <= q->index &&
bq->write_index + (int64_t) chunk.length >= q->index + (int64_t) q->chunk.length) {
/* This entry is fully replaced by the new entry, so let's drop it */
struct list_item *p;
p = q;
q = q->prev;
drop_block(bq, p);
} else if (bq->write_index >= q->index) {
/* The write index points into this memblock, so let's
* truncate or split it */
if (bq->write_index + (int64_t) chunk.length < q->index + (int64_t) q->chunk.length) {
/* We need to save the end of this memchunk */
struct list_item *p;
size_t d;
/* Create a new list entry for the end of the memchunk */
if (!(p = pa_flist_pop(PA_STATIC_FLIST_GET(list_items))))
p = pa_xnew(struct list_item, 1);
p->chunk = q->chunk;
pa_memblock_ref(p->chunk.memblock);
/* Calculate offset */
d = (size_t) (bq->write_index + (int64_t) chunk.length - q->index);
pa_assert(d > 0);
/* Drop it from the new entry */
p->index = q->index + (int64_t) d;
p->chunk.length -= d;
/* Add it to the list */
p->prev = q;
if ((p->next = q->next))
q->next->prev = p;
else
bq->blocks_tail = p;
q->next = p;
bq->n_blocks++;
}
/* Truncate the chunk */
if (!(q->chunk.length = (size_t) (bq->write_index - q->index))) {
struct list_item *p;
p = q;
q = q->prev;
drop_block(bq, p);
}
/* We had to truncate this block, hence we're now at the right position */
break;
} else {
size_t d;
pa_assert(bq->write_index + (int64_t)chunk.length > q->index &&
bq->write_index + (int64_t)chunk.length < q->index + (int64_t)q->chunk.length &&
bq->write_index < q->index);
/* The job overwrites the current entry at the end, so let's drop the beginning of this entry */
d = (size_t) (bq->write_index + (int64_t) chunk.length - q->index);
q->index += (int64_t) d;
q->chunk.index += d;
q->chunk.length -= d;
q = q->prev;
}
}
