void sumset_node_update(t_sumset_node *node)
{
static t_sumset_node const dummy = (t_sumset_node){SET_HEAD(), 0, 0, 0};
t_sumset_node const *const l = node->set_h.left ? node->set_h.left : &dummy;
t_sumset_node const *const r = node->set_h.right ? node->set_h.right : &dummy;
node->length_sum = l->length_sum + 1 + r->length_sum;
node->value_sum = l->value_sum + node->value + r->value_sum;
}
static void *
mb_realloc_(void *ptr, size_t size, const void *caller)
{
void *p;
struct mb_head *h = HEADER(ptr);
size_t oldsize;
LOCK();
RESTORE_HOOK();
realloc_called++;
if (!ptr) {
assert(size > 0);
if ((mem_limit && mb_allocated + size > mem_limit) ||
(call_limit && malloc_called + realloc_called > call_limit))
p = 0;
else
p = malloc(size + HEADER_SIZE);
if (p) {
mb_allocated += size;
add_stats(size);
SET_PEAK();
}
print_trace();
mb_log("%10zd: realloc(%p, %zu) from %p => %p", (ssize_t)size,
ptr, size, caller, p ? p + HEADER_SIZE : 0);
SET_HEAD(p, size);
SAVE_HOOK();
UNLOCK();
return p ? (unsigned char *)p + HEADER_SIZE : 0;
}
if (h->size == size) {
print_trace();
mb_log("%10zd: realloc(%p, %zu) from %p => %p", (ssize_t)0,
ptr, size, caller, p + HEADER_SIZE);
if (call_limit && malloc_called + realloc_called > call_limit)
ptr = 0;
SAVE_HOOK();
UNLOCK();
return ptr;
}
oldsize = h->size;
if ((mem_limit && mb_allocated + size - oldsize > mem_limit) ||
(call_limit && malloc_called + realloc_called > call_limit))
p = 0;
else
p = realloc((unsigned char *)ptr - HEADER_SIZE, size + HEADER_SIZE);
if (p) {
mb_allocated += size - oldsize;
add_stats(size);
SET_PEAK();
}
print_trace();
mb_log("%10zd: realloc(%p, %zu) from %p => %p",
(int)(size - oldsize), ptr, size, caller, p ? p + HEADER_SIZE : 0);
SET_HEAD(p, size - oldsize);
SAVE_HOOK();
UNLOCK();
return p ? (unsigned char *)p + HEADER_SIZE : 0;
}
bool TF20::process_cmd()
{
int drv, trk, sec, dst;
uint8 *sctr, *sctw;
switch(bufr[3]) {
case FNC_RESET_P:
case FNC_RESET_M:
SET_HEAD(0);
SET_CODE(ERR_SUCCESS);
return true;
case FNC_READ:
drv = (bufr[1] == DID_FIRST) ? 0 : (bufr[1] == DID_SECOND) ? 2 : 4;
drv += bufr[7] - 1;
trk = bufr[8];
sec = bufr[9];
if(!disk_inserted(drv)) {
// drive error
SET_HEAD(0x80);
for(int i = 0; i < 128; i++) {
SET_DATA(0xff);
}
SET_CODE(ERR_DRIVE);
return true;
}
if((sctr = get_sector(drv, trk, sec)) == NULL) {
// read error
SET_HEAD(0x80);
for(int i = 0; i < 128; i++) {
SET_DATA(0xff);
}
SET_CODE(ERR_READ);
return true;
}
SET_HEAD(0x80);
for(int i = 0; i < 128; i++) {
SET_DATA(sctr[i]);
}
SET_CODE(ERR_SUCCESS);
return true;
case FNC_WRITE:
drv = (bufr[1] == DID_FIRST) ? 0 : (bufr[1] == DID_SECOND) ? 2 : 4;
drv += bufr[7] - 1;
trk = bufr[8];
sec = bufr[9];
if(!disk_inserted(drv)) {
// drive error
SET_HEAD(0);
SET_CODE(ERR_DRIVE);
return true;
}
if(disk_protected(drv)) {
// write protect
SET_HEAD(0);
SET_CODE(ERR_PROTECTED);
return true;
}
if((sctw = get_sector(drv, trk, sec)) == NULL) {
// write error
SET_HEAD(0);
SET_CODE(ERR_WRITE);
return true;
}
// dont care write type
for(int i = 0; i < 128; i++) {
sctw[i] = bufr[11 + i];
}
SET_HEAD(0);
SET_CODE(ERR_SUCCESS);
return true;
case FNC_WRITEHST:
SET_HEAD(0);
SET_CODE(ERR_SUCCESS);
return true;
case FNC_COPY:
drv = (bufr[1] == DID_FIRST) ? 0 : (bufr[1] == DID_SECOND) ? 2 : 4;
drv += bufr[7] - 1;
dst = (drv & ~1) | (~drv & 1);
if(!disk_inserted(drv)) {
// drive error
SET_HEAD(0);
SET_CODE(ERR_DRIVE);
return true;
}
if(!disk_inserted(dst)) {
// drive error
SET_HEAD(0);
SET_CODE(ERR_DRIVE);
return true;
}
if(disk_protected(dst)) {
// write protect
SET_HEAD(0);
SET_CODE(ERR_PROTECTED);
return true;
}
for(trk = 0; trk < 40; trk++) {
for(sec = 1; sec <= 64; sec++) {
if((sctr = get_sector(drv, trk, sec)) == NULL) {
// read error
SET_HEAD(0);
SET_CODE(ERR_READ);
return true;
}
if((sctw = get_sector(dst, trk, sec)) == NULL) {
// write error
SET_HEAD(0);
SET_CODE(ERR_WRITE);
return true;
}
memcpy(sctw, sctr, 128);
}
SET_HEAD(2);
SET_DATA(trk == 39 ? 0xff : 0); // high-order
SET_DATA(trk == 39 ? 0xff : trk); // low-order
SET_CODE(ERR_SUCCESS);
}
return true;
case FNC_FORMAT:
drv = (bufr[1] == DID_FIRST) ? 0 : (bufr[1] == DID_SECOND) ? 2 : 4;
drv += bufr[7] - 1;
if(!disk_inserted(drv)) {
// drive error
SET_HEAD(0);
SET_CODE(ERR_DRIVE);
return true;
}
if(disk_protected(drv)) {
// write protect
SET_HEAD(0);
SET_CODE(ERR_PROTECTED);
return true;
}
for(trk = 0; trk < 40; trk++) {
for(sec = 1; sec <= 64; sec++) {
if((sctw = get_sector(drv, trk, sec)) == NULL) {
// write error
SET_HEAD(0);
SET_CODE(ERR_WRITE);
return true;
}
memset(sctw, 0xe5, 128);
}
SET_HEAD(2);
SET_DATA(trk == 39 ? 0xff : 0); // high-order
SET_DATA(trk == 39 ? 0xff : trk); // low-order
SET_CODE(ERR_SUCCESS);
}
return true;
}
// unknown command
return false;
}