/*
check one register value
*/
bool AP_HAL::Device::check_next_register(void)
{
if (_checked.n_set == 0) {
return true;
}
if (++_checked.counter < _checked.frequency) {
return true;
}
_checked.counter = 0;
struct checkreg ® = _checked.regs[_checked.next];
uint8_t v;
if (!read_registers(reg.regnum, &v, 1) || v != reg.value) {
// a register has changed value unexpectedly. Try changing it back
// and re-check it next time
#if 0
printf("Device 0x%x fixing 0x%02x 0x%02x -> 0x%02x\n",
(unsigned)get_bus_id(),
(unsigned)reg.regnum, (unsigned)v, (unsigned)reg.value);
#endif
write_register(reg.regnum, reg.value);
return false;
}
_checked.next = (_checked.next+1) % _checked.n_set;
return true;
}
/* Attach to PID `pid', take a snapshot, modify its state to have it call
* `dlopen()', restore the previously saved snapshot and detach.
*/
static int inject(pid_t pid, char *filename, char all_thrs)
{
regs_t regs;
char buf[PAGE_SIZE];
ssize_t size;
int r = -1;
if(attach(pid, all_thrs) != 0)
goto ret;
if(read_registers(pid, ®s) != 0)
goto ret;
if((size = read_memory(pid, (void *)SP(regs), buf, sizeof(buf))) < 0)
goto ret;
r = 0;
if(force_dlopen(pid, filename) != 0)
r = -1;
if(write_memory(pid, (void *)SP(regs), buf, size) != size)
r = -1;
if(write_registers(pid, ®s) != 0)
r = -1;
if(detach(pid, all_thrs) != 0)
r = -1;
ret:
return r;
}
int main( int argc, char *argv[]) {
int fd;
fd=open_chip();
read_registers(fd);
close_chip(fd);
reg[0x02] |= (1<<10); // allow wrap
reg[0x02] |= (1<<9); // set seek direction
reg[0x02] |= (1<<8); // start seek
reg[0x06] |= (1<<4); // set min seek threshold
reg[0x06] |= (1<<1); //set min FM Imp. threshold
write_registers();
// watch for STC == 1
while (1) {
fd=open_chip();
read_registers(fd);
close_chip(fd);
if((reg[10] & (1<<14)) !=0) break; //tuning finished
}
fd=open_chip();
read_registers(fd);
close_chip(fd);
int SFBL = reg[0x0A] & (1<<13);
reg[0x02] &= ~(1<<8); //clear the seek bit
write_registers();
// usleep(80000);
while (1) {
fd=open_chip();
read_registers(fd);
close_chip(fd);
if((reg[0x0A] & (1<<14)) == 0) break;
}
if(SFBL) {
exit(1);
}
return;
}
static int process_gdb_command(struct gdb_data *data, char *buf, int len)
{
#ifdef DEBUG_GDB
printc("process_gdb_command: %s\n", buf);
#endif
switch (buf[0]) {
case '?': /* Return target halt reason */
return run_final_status(data);
case 'z':
case 'Z':
return set_breakpoint(data, buf[0] == 'Z', buf + 1);
case 'r': /* Restart */
case 'R':
return restart_program(data);
case 'g': /* Read registers */
return read_registers(data);
case 'G': /* Write registers */
return write_registers(data, buf + 1);
case 'q': /* Query */
if (!strncmp(buf, "qRcmd,", 6))
return monitor_command(data, buf + 6);
if (!strncmp(buf, "qSupported", 10))
return gdb_send_supported(data);
break;
case 'm': /* Read memory */
return read_memory(data, buf + 1);
case 'M': /* Write memory */
return write_memory(data, buf + 1);
case 'c': /* Continue */
return run(data, buf + 1);
case 's': /* Single step */
return single_step(data, buf + 1);
case 'k': /* kill */
return -1;
}
#ifdef DEBUG_GDB
printc("process_gdb_command: unknown command %s\n", buf);
#endif
/* For unknown/unsupported packets, return an empty reply */
return gdb_send(data, "");
}
/*
check a SPI device for a register value
*/
bool AP_BoardConfig::spi_check_register(const char *devname, uint8_t regnum, uint8_t value, uint8_t read_flag)
{
auto dev = hal.spi->get_device(devname);
if (!dev) {
hal.console->printf("%s: no device\n", devname);
return false;
}
dev->set_read_flag(read_flag);
uint8_t v;
if (!dev->read_registers(regnum, &v, 1)) {
hal.console->printf("%s: reg %02x read fail\n", devname, (unsigned)regnum);
return false;
}
hal.console->printf("%s: reg %02x expected:%02x got:%02x\n", devname, (unsigned)regnum, (unsigned)value, (unsigned)v);
return v == value;
}
void readAccelData(short int *destination)
{
uint8_t rawData[6]; // x/y/z accel register data stored here
bool init = true;
init &= read_registers(MMA8452_ADDRESS << 1,OUT_X_MSB, 6, rawData); // Read the six raw data registers into data array
// ACC_getdata(rawData);
nrf_delay_ms(1);
char c[30];
sprintf(c,"%d",rawData[0]);
simple_uart_putstring((const uint8_t *) "\r\n");
simple_uart_putstring((const uint8_t *) c);
sprintf(c,"%d",rawData[1]);
simple_uart_putstring((const uint8_t *) " ");
simple_uart_putstring((const uint8_t *) c);
sprintf(c,"%d",rawData[2]);
simple_uart_putstring((const uint8_t *) " ");
simple_uart_putstring((const uint8_t *) c);
sprintf(c,"%d",rawData[3]);
simple_uart_putstring((const uint8_t *) " ");
simple_uart_putstring((const uint8_t *) c);
sprintf(c,"%d",rawData[4]);
simple_uart_putstring((const uint8_t *) " ");
simple_uart_putstring((const uint8_t *) c);
sprintf(c,"%d",rawData[5]);
simple_uart_putstring((const uint8_t *) " ");
simple_uart_putstring((const uint8_t *) c);
// Loop to calculate 12-bit ADC and g value for each axis
for(int i = 0; i < 3 ; i++)
{
short int gCount = (rawData[i*2] << 8) | rawData[(i*2)+1]; //Combine the two 8 bit registers into one 12-bit number
gCount = gCount >> 4; //The registers are left align, here we right align the 12-bit integer
// // If the number is negative, we have to make it so manually (no 12-bit data type)
if (rawData[i*2] > 0x7F)
{
gCount = ~gCount + 1;
gCount *= -1; // Transform into negative 2's complement #
}
destination[i] = gCount; //Record this gCount into the 3 int array
}
}
/* Reads the input registers of remote device and put the data into an array */
int modbus_read_input_registers(modbus_t *ctx, int addr, int nb,
uint16_t *dest)
{
int status;
if (nb > MODBUS_MAX_READ_REGISTERS) {
fprintf(stderr,
"ERROR Too many input registers requested (%d > %d)\n",
nb, MODBUS_MAX_READ_REGISTERS);
errno = EMBMDATA;
return -1;
}
status = read_registers(ctx, _FC_READ_INPUT_REGISTERS,
addr, nb, dest);
return status;
}
/* Reads the holding registers of remote device and put the data into an
array */
DLL int modbus_read_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest)
{
int status;
if (nb > MODBUS_MAX_READ_REGISTERS) {
if (ctx->debug) {
fprintf(stderr,
"ERROR Too many registers requested (%d > %d)\n",
nb, MODBUS_MAX_READ_REGISTERS);
}
errno = EMBMDATA;
return -1;
}
status = read_registers(ctx, _FC_READ_HOLDING_REGISTERS,
addr, nb, dest);
return status;
}
int check_active_psus() {
int error = 0;
lock_holder(worldlock, &world.lock);
lock_take(worldlock);
if (world.paused == 1) {
usleep(1000);
goto cleanup;
}
if (world.config == NULL) {
lock_release(worldlock);
usleep(5000);
goto cleanup;
}
world.num_active_addrs = 0;
scanning = 1;
//fprintf(stderr, "Begin presence check: ");
for(int rack = 0; rack < 3; rack++) {
for(int shelf = 0; shelf < 2; shelf++) {
for(int psu = 0; psu < 3; psu++) {
char addr = psu_address(rack, shelf, psu);
uint16_t status = 0;
int err = read_registers(&world.rs485, world.modbus_timeout, addr, REGISTER_PSU_STATUS, 1, &status);
if (err == 0) {
world.active_addrs[world.num_active_addrs] = addr;
world.num_active_addrs++;
//fprintf(stderr, "%02x - active (%04x) ", addr, status);
} else {
dbg("%02x - %d; ", addr, err);
}
}
}
}
//its the only stdlib sort
qsort(world.active_addrs, world.num_active_addrs,
sizeof(uint8_t), sub_uint8s);
cleanup:
scanning = 0;
lock_release(worldlock);
return error;
}
/*
* Find/display global/local variables which own the most heap memory in bytes
*/
CA_BOOL biggest_heap_owners_generic(unsigned int num, CA_BOOL all_reachable_blocks)
{
CA_BOOL rc = CA_FALSE;
unsigned int i;
int nregs = 0;
struct reg_value *regs_buf = NULL;
size_t ptr_sz = g_ptr_bit >> 3;
struct heap_owner *owners;
struct heap_owner *smallest;
struct ca_segment *segment;
size_t total_bytes = 0;
size_t processed_bytes = 0;
struct inuse_block *inuse_blocks = NULL;
unsigned long num_inuse_blocks;
unsigned long inuse_index;
struct inuse_block *blk;
struct object_reference ref;
size_t aggr_size;
unsigned long aggr_count;
address_t start, end, cursor;
// Allocate an array for the biggest num of owners
if (num == 0)
return CA_FALSE;
owners = (struct heap_owner *) calloc(num, sizeof(struct heap_owner));
if (!owners)
goto clean_out;
smallest = &owners[num - 1];
// First, create and populate an array of all in-use blocks
inuse_blocks = build_inuse_heap_blocks(&num_inuse_blocks);
if (!inuse_blocks || num_inuse_blocks == 0)
{
CA_PRINT("Failed: no in-use heap block is found\n");
goto clean_out;
}
// estimate the work to enable progress bar
for (i=0; i<g_segment_count; i++)
{
segment = &g_segments[i];
if (segment->m_type == ENUM_STACK || segment->m_type == ENUM_MODULE_DATA)
total_bytes += segment->m_fsize;
}
init_progress_bar(total_bytes);
// Walk through all segments of threads' registers/stacks or globals
for (i=0; i<g_segment_count; i++)
{
// bail out if user is impatient for the long searching
if (user_request_break())
{
CA_PRINT("Abort searching biggest heap memory owners\n");
goto clean_out;
}
// Only thread stack and global .data sections are considered
segment = &g_segments[i];
if (segment->m_type == ENUM_STACK || segment->m_type == ENUM_MODULE_DATA)
{
int tid = 0;
// check registers if it is a thread's stack segment
if (segment->m_type == ENUM_STACK)
{
tid = get_thread_id (segment);
// allocate register value buffer for once
if (!nregs && !regs_buf)
{
nregs = read_registers (NULL, NULL, 0);
if (nregs)
regs_buf = (struct reg_value*) malloc(nregs * sizeof(struct reg_value));
}
// check each register for heap reference
if (nregs && regs_buf)
{
int k;
int nread = read_registers (segment, regs_buf, nregs);
for (k = 0; k < nread; k++)
{
if (regs_buf[k].reg_width == ptr_sz)
{
blk = find_inuse_block(regs_buf[k].value, inuse_blocks, num_inuse_blocks);
if (blk)
{
ref.storage_type = ENUM_REGISTER;
ref.vaddr = 0;
ref.value = blk->addr;
ref.where.reg.tid = tid;
ref.where.reg.reg_num = k;
ref.where.reg.name = NULL;
calc_aggregate_size(&ref, ptr_sz, all_reachable_blocks, inuse_blocks, num_inuse_blocks, &aggr_size, &aggr_count);
if (aggr_size > smallest->aggr_size)
{
struct heap_owner newowner;
newowner.ref = ref;
newowner.aggr_size = aggr_size;
newowner.aggr_count = aggr_count;
add_owner(owners, num, &newowner);
}
}
}
}
}
}
// Calculate the memory region to search
if (segment->m_type == ENUM_STACK)
{
start = get_rsp(segment);
if (start < segment->m_vaddr || start >= segment->m_vaddr + segment->m_vsize)
start = segment->m_vaddr;
if (start - segment->m_vaddr >= segment->m_fsize)
end = start;
else
end = segment->m_vaddr + segment->m_fsize;
}
else if (segment->m_type == ENUM_MODULE_DATA)
{
start = segment->m_vaddr;
end = segment->m_vaddr + segment->m_fsize;
}
else
continue;
// Evaluate each variable or raw pointer in the target memory region
cursor = ALIGN(start, ptr_sz);
while (cursor < end)
{
size_t val_len = ptr_sz;
address_t sym_addr;
size_t sym_sz;
CA_BOOL known_sym = CA_FALSE;
// If the address belongs to a known variable, include all its subfields
// FIXME
// consider subfields that are of pointer-like types, however, it will miss
// references in an unstructured buffer
ref.storage_type = segment->m_type;
ref.vaddr = cursor;
if (segment->m_type == ENUM_STACK)
{
ref.where.stack.tid = tid;
ref.where.stack.frame = get_frame_number(segment, cursor, &ref.where.stack.offset);
if (known_stack_sym(&ref, &sym_addr, &sym_sz) && sym_sz)
known_sym = CA_TRUE;
}
else if (segment->m_type == ENUM_MODULE_DATA)
{
ref.where.module.base = segment->m_vaddr;
ref.where.module.size = segment->m_vsize;
ref.where.module.name = segment->m_module_name;
if (known_global_sym(&ref, &sym_addr, &sym_sz) && sym_sz)
known_sym = CA_TRUE;
}
if (known_sym)
{
if (cursor != sym_addr)
ref.vaddr = cursor = sym_addr; // we should never come to here!
val_len = sym_sz;
}
// Query heap for aggregated memory size/count originated from the candidate variable
if (val_len >= ptr_sz)
{
calc_aggregate_size(&ref, val_len, all_reachable_blocks, inuse_blocks, num_inuse_blocks, &aggr_size, &aggr_count);
// update the top list if applies
if (aggr_size >= smallest->aggr_size)
{
struct heap_owner newowner;
if (val_len == ptr_sz)
read_memory_wrapper(NULL, ref.vaddr, (void*)&ref.value, ptr_sz);
else
ref.value = 0;
newowner.ref = ref;
newowner.aggr_size = aggr_size;
newowner.aggr_count = aggr_count;
add_owner(owners, num, &newowner);
}
}
cursor = ALIGN(cursor + val_len, ptr_sz);
}
processed_bytes += segment->m_fsize;
set_current_progress(processed_bytes);
}
}
end_progress_bar();
if (!all_reachable_blocks)
{
// Big memory blocks may be referenced indirectly by local/global variables
// check all in-use blocks
for (inuse_index = 0; inuse_index < num_inuse_blocks; inuse_index++)
{
blk = &inuse_blocks[inuse_index];
ref.storage_type = ENUM_HEAP;
ref.vaddr = blk->addr;
ref.where.heap.addr = blk->addr;
ref.where.heap.size = blk->size;
ref.where.heap.inuse = 1;
calc_aggregate_size(&ref, ptr_sz, CA_FALSE, inuse_blocks, num_inuse_blocks, &aggr_size, &aggr_count);
// update the top list if applies
if (aggr_size >= smallest->aggr_size)
{
struct heap_owner newowner;
ref.value = 0;
newowner.ref = ref;
newowner.aggr_size = aggr_size;
newowner.aggr_count = aggr_count;
add_owner(owners, num, &newowner);
}
}
}
// Print the result
for (i = 0; i < num; i++)
{
struct heap_owner *owner = &owners[i];
if (owner->aggr_size)
{
CA_PRINT("[%d] ", i+1);
print_ref(&owner->ref, 0, CA_FALSE, CA_FALSE);
CA_PRINT(" |--> ");
print_size(owner->aggr_size);
CA_PRINT(" (%ld blocks)\n", owner->aggr_count);
}
}
rc = CA_TRUE;
clean_out:
// clean up
if (regs_buf)
free (regs_buf);
if (owners)
free (owners);
if (inuse_blocks)
free_inuse_heap_blocks (inuse_blocks, num_inuse_blocks);
return rc;
}
/* Force the target to call `dlopen()'. Modify its registers and stack contents
* and continue execution until a segmentation fault is caught. Return 0 on
* success and -1 on failure.
*/
static int force_dlopen(pid_t pid, char *filename)
{
void *linker_addr, *dlopen_off, *dlopen_addr;
regs_t regs;
size_t size;
int r = -1;
if(resolve_symbol(LINKER_PATH, DLOPEN_NAME1, &dlopen_off) != 0 &&
resolve_symbol(LINKER_PATH, DLOPEN_NAME2, &dlopen_off) != 0 &&
resolve_symbol(LINKER_PATH, DLOPEN_NAME3, &dlopen_off) != 0)
{
printf("[*] Could not resolve dlopen()\n");
goto ret;
}
if((linker_addr = get_linker_addr(pid)) == NULL)
{
printf("[*] Linker not found in PID %d\n", pid);
goto ret;
}
dlopen_addr = linker_addr + (ptrdiff_t)dlopen_off;
printf("[*] Resolved dlopen() at %p\n", dlopen_addr);
if(read_registers(pid, ®s) != 0)
goto ret;
/* Prepare `do_dlopen()' input arguments. On Android >= 7, we set the 4th
* argument to a value that emulates `__builtin_return_address()', so that
* our DSO is loaded in the correct namespace.
*/
ARG0(regs) = SP(regs) + SP_OFF;
ARG1(regs) = RTLD_NOW | RTLD_GLOBAL;
ARG2(regs) = 0;
ARG3(regs) = PC(regs);
/* We set the new PC and also set LR to force the debuggee to crash. */
#if defined(__aarch64__)
LR(regs) = 0xffffffffffffffff;
PC(regs) = (reg_t)dlopen_addr;
#elif defined(__arm__)
LR(regs) = 0xffffffff;
PC(regs) = (reg_t)dlopen_addr | 1;
CPSR(regs) |= 1 << 5;
#endif
printf("[*] Modifying target's state\n");
if(write_registers(pid, ®s) != 0)
goto ret;
size = strlen(filename) + 1;
if(write_memory(pid, (void *)SP(regs) + SP_OFF, filename, size) != size)
goto ret;
printf("[*] Waiting for target to throw SIGSEGV or SIGBUS\n");
if(resume_and_wait(pid) != 0)
goto ret;
r = 0;
ret:
return r;
}
/* return value: 0 if the simulator is to be killed,
* 1 if the simulator is to be continued.
*/
static int process_gdb_loop(void)
{
int addr;
int length;
int cpu_id;
int step_cpu, other_cpu = 0;
char *ptr;
char type;
int regnum;
uint32_t val;
regnum = regnum;
step_cpu = other_cpu = 0;
/* if the hardware is running, we dropped here because the user has
* hit break in gdb, so we send a signal to GDB indicating that */
if (hardware->running == 1) {
remcomOutBuffer[0] = 'S';
remcomOutBuffer[1] = '0';
remcomOutBuffer[2] = '5';
remcomOutBuffer[3] = 0;
putpacket((unsigned char *)remcomOutBuffer);
}
while (1)
{
remcomOutBuffer[0] = 0;
ptr = (char*)getpacket();
if (ptr == NULL) {
/* we didn't receive a valid packet, assume that
the connection has been terminated */
gdb_interface_close();
return 1;
}
if (debug_packets) printf("from gdb:%s\n", ptr);
switch (*ptr++) {
case '?': /* `?' -- last signal */
remcomOutBuffer[0] = 'S';
remcomOutBuffer[1] = '0';
remcomOutBuffer[2] = '1';
remcomOutBuffer[3] = 0;
break;
case 'c': /* cAA..AA Continue at address AA..AA(optional) */
if (hexToInt(&ptr, &addr))
set_pc(step_cpu, addr);
hardware->running = 1;
return 1;
break;
case 'd': /* `d' -- toggle debug *(deprecated)* */
debug_packets = (debug_packets + 1) % 2;
break;
case 'g': /* return the value of the CPU registers */
read_registers(other_cpu, remcomOutBuffer);
break;
case 'G': /* set the value of the CPU registers - return OK */
write_registers(other_cpu, ptr);
strcpy(remcomOutBuffer,"OK");
break;
case 'H': /* `H'CT... -- set thread */
type = *ptr++;
if (hexToInt(&ptr, &cpu_id)) {
if (cpu_id == -1 || cpu_id == 0) /* XXX all threads */
cpu_id = 1;
if (type == 'c') {
step_cpu = cpu_id - 1; /* minus one because
gdb threats start from 1
and yams cpu's from 0. */
strcpy(remcomOutBuffer, "OK");
} else if (type == 'g') {
other_cpu = cpu_id - 1; /* same here */
strcpy(remcomOutBuffer, "OK");
} else
strcpy(remcomOutBuffer, "E01");
} else
strcpy(remcomOutBuffer, "E01");
break;
case 'k' : /* kill the program */
return 0;
break;
case 'm': /* mAA..AA,LLLL Read LLLL bytes at address AA..AA */
if (hexToInt(&ptr,&addr) &&*ptr++==','&& hexToInt(&ptr,&length)) {
if (read_mem(addr, length, remcomOutBuffer))
strcpy(remcomOutBuffer, "E03");
} else
strcpy(remcomOutBuffer, "E01");
break;
case 'M': /* MAA..AA,LLLL: Write LLLL bytes at address AA.AA */
if (hexToInt(&ptr, &addr) && *ptr++ == ','
&& hexToInt(&ptr, &length) && *ptr++ == ':') {
if (!write_mem(addr, length, ptr))
strcpy(remcomOutBuffer, "OK");
else
strcpy(remcomOutBuffer, "E03");
}
else
strcpy(remcomOutBuffer, "E02");
break;
case 'p': /* `p'HEX NUMBER OF REGISTER -- read register packet */
if (hexToInt(&ptr, ®num) && regnum <= 73)
sprintf(remcomOutBuffer, "%08x",
read_register(other_cpu, regnum));
else
sprintf(remcomOutBuffer, "E01");
break;
case 'P': /* `P'N...`='R... -- write register */
if (hexToInt(&ptr, (int*)®num) && *ptr++=='=' && hexToInt(&ptr, (int*)&val)) {
write_register(other_cpu, regnum, val);
sprintf(remcomOutBuffer, "OK");
} else
sprintf(remcomOutBuffer, "E01");
case 'q': /* `q'QUERY -- general query */
if (!strcmp(ptr, "fThreadInfo")) {
int i;
char *ptr = remcomOutBuffer;
ptr += sprintf(ptr, "m01");
if (hardware->num_cpus > 1)
for (i = 1; i < hardware->num_cpus; i++)
ptr += sprintf(ptr, ",%02x", i + 1);
sprintf(ptr, "l");
}
break;
case 's': /* `s'ADDR -- step */
command_step(1);
sprintf(remcomOutBuffer, "S01");
break;
case 'T': /* `T'XX -- thread alive */
if (hexToInt(&ptr, &cpu_id) && --cpu_id < hardware->num_cpus)
strcpy(remcomOutBuffer, "OK");
else
strcpy(remcomOutBuffer, "E01");
break;
case 'z': /* remove breakpoint: `Z'TYPE`,'ADDR`,'LENGTH */
type = *ptr++;
if (*ptr++== ',' && hexToInt(&ptr, &addr)
&& *ptr++ == ',' && hexToInt(&ptr, &length)) {
if (type == '1') { /* hardware breakpoint */
command_breakpoint(0xFFFFFFFF);
strcpy(remcomOutBuffer, "OK");
} else /* all others are unsupported */
strcpy(remcomOutBuffer, "E01");
} else
strcpy(remcomOutBuffer, "E02");
break;
case 'Z': /* insert breakpoint: `Z'TYPE`,'ADDR`,'LENGTH */
type = *ptr++;
if (*ptr++== ',' && hexToInt(&ptr, &addr)
&& *ptr++ == ',' && hexToInt(&ptr, &length)) {
if (type == '1') { /* hardware breakpoint */
command_breakpoint(addr);
strcpy(remcomOutBuffer, "OK");
} else /* all others are unsupported */
strcpy(remcomOutBuffer, "E01");
} else
strcpy(remcomOutBuffer, "E02");
break;
default:
break;
} /* switch */
/* reply to the request */
putpacket((unsigned char *)remcomOutBuffer);
if (debug_packets) printf("to gdb: %s\n", remcomOutBuffer);
}
}
bool read_register(uint8_t addr, uint8_t reg, uint8_t& data)
{
return read_registers(addr, reg, &data, 1);
}
int fetch_monitored_data() {
int error = 0;
int data_pos = 0;
lock_holder(worldlock, &world.lock);
lock_take(worldlock);
if (world.paused == 1) {
usleep(1000);
goto cleanup;
}
if (world.config == NULL) {
goto cleanup;
}
lock_release(worldlock);
usleep(1000); // wait a sec btween PSUs to not overload RT scheduling
// threshold
while(world.stored_data[data_pos] != NULL && data_pos < MAX_ACTIVE_ADDRS) {
uint8_t addr = world.stored_data[data_pos]->addr;
//log("readpsu %02x\n", addr);
for(int r = 0; r < world.config->num_intervals; r++) {
register_range_data* rd = &world.stored_data[data_pos]->range_data[r];
monitor_interval* i = rd->i;
uint16_t regs[i->len];
int err = read_registers(&world.rs485,
world.modbus_timeout, addr, i->begin, i->len, regs);
if (err) {
log("Error %d reading %02x registers at %02x from %02x\n",
err, i->len, i->begin, addr);
continue;
}
struct timespec ts;
clock_gettime(CLOCK_REALTIME, &ts);
uint32_t timestamp = ts.tv_sec;
if (rd->i->flags & MONITOR_FLAG_ONLY_CHANGES) {
int pitch = sizeof(timestamp) + (sizeof(uint16_t) * i->len);
int lastpos = rd->mem_pos - pitch;
if (lastpos < 0) {
lastpos = (pitch * rd->i->keep) - pitch;
}
if (!memcmp(rd->mem_begin + lastpos + sizeof(timestamp),
regs, sizeof(uint16_t) * i->len) &&
memcmp(rd->mem_begin, "\x00\x00\x00\x00", 4)) {
continue;
}
if (world.status_log) {
time_t rawt;
struct tm* ti;
time(&rawt);
ti = localtime(&rawt);
char timestr[80];
strftime(timestr, sizeof(timestr), "%b %e %T", ti);
fprintf(world.status_log,
"%s: Change to status register %02x on address %02x. New value: %02x\n",
timestr, i->begin, addr, regs[0]);
fflush(world.status_log);
}
}
lock_take(worldlock);
record_data(rd, timestamp, regs);
lock_release(worldlock);
}
data_pos++;
}
cleanup:
lock_release(worldlock);
return error;
}