double lps331_temperature(int fd) {
uint8_t l, h;
if (read_at(fd, LPS331_ADDR_TEMP_L, &l, 1) <= 0) {
return -10000;
}
if (read_at(fd, LPS331_ADDR_TEMP_H, &h, 1) <= 0) {
return -10000;
}
return 42.5 + ((int16_t) ((h << 8) | l)) / 480.0;
}
double lps331_pressure(int fd) {
uint8_t l, h, xl;
if (read_at(fd, LPS331_ADDR_PRESS_XL, &xl, 1) <= 0) {
return 0;
}
if (read_at(fd, LPS331_ADDR_PRESS_L, &l, 1) <= 0) {
return 0;
}
if (read_at(fd, LPS331_ADDR_PRESS_H, &h, 1) <= 0) {
return 0;
}
return ((uint32_t) ((h << 16) | (l << 8) | (xl))) / 4096.0;
}
bool mag3110_check_device(int fd) {
uint8_t id;
if (read_at(fd, MAG3110_ADDR_WHO_AM_I, &id, 1) < 0) {
return false;
}
return id == MAG3110_ID;
}
static int find_interp_name(elf_bin_t *bin, char *buf, unsigned long max_size)
{
long i;
Elf32_Phdr *p;
for (i=0; i<bin->hdr.e_phnum; i++)
{
p = &bin->phdr[i];
if (p->p_type == PT_INTERP)
{
if ( (p->p_filesz > max_size) || (p->p_filesz < 2) )
return -ENOEXEC;
if ( read_at(bin->fd, p->p_offset, buf,
p->p_filesz) != (long)p->p_filesz )
return -EIO;
if ( buf[p->p_filesz-1] != '\0' )
return -ENOEXEC;
return 1;
}
}
return 0;
}
bool lps331_check_device(int fd) {
uint8_t id;
if (read_at(fd, LPS331_ADDR_WHO_AM_I, &id, 1) < 0) {
return false;
}
return id == LPS331_ID;
}
uint8_t lps331_status(int fd) {
uint8_t v;
if (read_at(fd, LPS331_ADDR_STATUS_REG, &v, 1) < 1) {
return 0;
}
return v;
}
int8_t mag3110_temperature(int fd) {
int8_t value;
if (read_at(fd, MAG3110_ADDR_DIE_TEMP, &value, sizeof(value)) < 0) {
return -127;
};
return value;
}
void process_program_header_note(Elf32_Phdr *phdr) {
long desc_offset, offset;
struct {
int name_size;
int desc_size;
int type;
} note_header;
offset = phdr->p_offset;
while (offset < phdr->p_offset + phdr->p_filesz) {
read_at(offset, ¬e_header, sizeof(note_header));
// offset at current note
offset += sizeof(note_header);
// offset at name
offset += note_header.name_size;
offset = ALIGN4(offset);
// offset at desc
desc_offset = offset;
offset += note_header.desc_size;
offset = ALIGN4(offset);
// offset at next note
switch (note_header.type) {
case NT_PRSTATUS:
if (notes_found & NOTE_FOUND_PRSTATUS)
error("Multiple NT_PRSTATUS notes.");
notes_found |= NOTE_FOUND_PRSTATUS;
read_at(desc_offset, &prstatus, sizeof(prstatus));
break;
case NT_FILE:
if (notes_found & NOTE_FOUND_FILE)
error("Multiple NT_FILE notes.");
notes_found |= NOTE_FOUND_FILE;
process_note_file(desc_offset);
break;
case NT_386_TLS:
if (notes_found & NOTE_FOUND_386_TLS)
error("Multiple NT_386_TLS notes.");
notes_found |= NOTE_FOUND_386_TLS;
read_at(desc_offset, &user_desc, sizeof(user_desc));
break;
}
}
}
inline std::size_t read_at(SyncRandomAccessReadDevice& d,
uint64_t offset, lslboost::asio::basic_streambuf<Allocator>& b)
{
lslboost::system::error_code ec;
std::size_t bytes_transferred = read_at(
d, offset, b, transfer_all(), ec);
lslboost::asio::detail::throw_error(ec, "read_at");
return bytes_transferred;
}
inline std::size_t read_at(SyncRandomAccessReadDevice& d,
uint64_t offset, const MutableBufferSequence& buffers)
{
lslboost::system::error_code ec;
std::size_t bytes_transferred = read_at(
d, offset, buffers, transfer_all(), ec);
lslboost::asio::detail::throw_error(ec, "read_at");
return bytes_transferred;
}
inline std::size_t read_at(SyncRandomAccessReadDevice& d,
boost::uint64_t offset, asio::basic_streambuf<Allocator>& b,
CompletionCondition completion_condition)
{
asio::error_code ec;
std::size_t bytes_transferred = read_at(
d, offset, b, completion_condition, ec);
asio::detail::throw_error(ec, "read_at");
return bytes_transferred;
}
inline std::size_t read_at(SyncRandomAccessReadDevice& d,
boost::uint64_t offset, const MutableBufferSequence& buffers,
CompletionCondition completion_condition)
{
asio::error_code ec;
std::size_t bytes_transferred = read_at(
d, offset, buffers, completion_condition, ec);
asio::detail::throw_error(ec, "read_at");
return bytes_transferred;
}
void for_each_program_header(Elf32_Word type, void (*func)(Elf32_Phdr*)) {
int i;
Elf32_Phdr phdr;
for (i = 0; i < hdr.e_phnum; ++i) {
read_at(hdr.e_phoff + i * hdr.e_phentsize, &phdr,
sizeof(Elf32_Phdr));
if (phdr.p_type == type)
(*func)(&phdr);
}
}
bool DataSource::getUInt16(off_t offset, uint16_t *x) {
*x = 0;
uint8_t byte[2];
if (read_at(offset, byte, 2) != 2) {
return false;
}
*x = (byte[0] << 8) | byte[1];
return true;
}
void do_raise() {
/* unmap old stack */
munmap((void*) 0x8000000, 0xc0000000 - 0x8000000);
/* open file */
fd = open(filename, O_RDONLY);
if (fd < 0) error("Failed to open the file.");
/* read and verify header */
if (read_at(0, &hdr, sizeof(Elf32_Ehdr)) != sizeof(Elf32_Ehdr)
|| hdr.e_ident[EI_MAG0] != ELFMAG0
|| hdr.e_ident[EI_MAG1] != ELFMAG1
|| hdr.e_ident[EI_MAG2] != ELFMAG2
|| hdr.e_ident[EI_MAG3] != ELFMAG3)
error("Not an ELF.");
if (hdr.e_type != ET_CORE)
error("Not a core ELF.");
if (hdr.e_machine != EM_386)
error("Bad machine.");
if (hdr.e_version != EV_CURRENT)
error("Bad version.");
/* extract relevant info from notes */
for_each_program_header(PT_NOTE, process_program_header_note);
if ((notes_found & NOTE_FOUND_ALL_RELEVANT) != NOTE_FOUND_ALL_RELEVANT)
error("Relevant notes missing.");
/* set up memory */
for_each_program_header(PT_LOAD, process_program_header_load);
/* close file */
close(fd);
/* prepare registers */
eax = prstatus.pr_reg[6];
ebx = prstatus.pr_reg[0];
ecx = prstatus.pr_reg[1];
edx = prstatus.pr_reg[2];
esi = prstatus.pr_reg[3];
edi = prstatus.pr_reg[4];
ebp = prstatus.pr_reg[5];
esp = prstatus.pr_reg[15];
eip = prstatus.pr_reg[12];
eflags = prstatus.pr_reg[14];
/* tls */
if (syscall(SYS_set_thread_area, &user_desc) < 0)
error("tls");
/* set registers */
set_registers();
error("unreachable");
}
void process_note_file(long note_file_desc_offset) {
long entry_offset, filename_offset;
int i;
read_at(note_file_desc_offset, ¬e_file_header,
sizeof(note_file_header));
entry_offset = note_file_desc_offset + sizeof(note_file_header);
filename_offset = entry_offset
+ note_file_header.count * NOTE_FILE_ENTRY_SIZE ;
for (i = 0; i < note_file_header.count; ++i) {
read_at(entry_offset, ¬e_file_files[i],
NOTE_FILE_ENTRY_SIZE);
read_at(filename_offset, note_file_files[i].filename,
FILENAME_SIZE);
note_file_files[i].filename[FILENAME_SIZE - 1] = '\0';
entry_offset += NOTE_FILE_ENTRY_SIZE;
filename_offset += strlen(note_file_files[i].filename) + 1;
}
}
void process_program_header_load(Elf32_Phdr *phdr) {
int i, b_fd, prot;
struct note_file_file *b_file = NULL;
void *mem;
/* check if there is a backing file */
for (i = 0; i < note_file_header.count; ++i)
if (phdr->p_vaddr == note_file_files[i].start)
b_file = ¬e_file_files[i];
/* map memory */
if (b_file == NULL) {
mem = mmap((void*) phdr->p_vaddr, phdr->p_memsz, PROT_WRITE,
MAP_PRIVATE | MAP_FIXED | MAP_ANONYMOUS, 0, 0);
} else {
b_fd = open(b_file->filename, O_RDONLY);
if (b_fd == -1)
error("open mapped file");
mem = mmap((void*) phdr->p_vaddr, phdr->p_memsz, PROT_WRITE,
MAP_PRIVATE | MAP_FIXED,
b_fd,
b_file->pgoff * note_file_header.page_size);
close(b_fd);
}
if (mem == MAP_FAILED)
error("mmap");
if (mem != (void*) phdr->p_vaddr)
error("mmap at requested address");
/* copy data */
read_at(phdr->p_offset, mem, phdr->p_filesz);
/* protect memory */
prot = PROT_NONE
| (phdr->p_flags & PF_MASKOS)
| (phdr->p_flags & PF_MASKPROC);
if (phdr->p_flags & PF_R)
prot = prot | PROT_READ;
if (phdr->p_flags & PF_W)
prot = prot | PROT_WRITE;
if (phdr->p_flags & PF_X)
prot = prot | PROT_EXEC;
if (mprotect(mem, phdr->p_memsz, prot) == -1)
error("mprotect");
}
static int read_elf_header(int fd, Elf32_Ehdr *hdr)
{
ssize_t sz = read_at(fd, 0, hdr, sizeof(*hdr));
if (sz < 0)
return sz;
if (sz != sizeof(*hdr))
return -EIO;
if ( (memcmp(&hdr[0], ELFMAG, SELFMAG) != 0) ||
((hdr->e_type != ET_EXEC) &&
(hdr->e_type != ET_DYN)) ||
((hdr->e_machine != EM_386) &&
(hdr->e_machine != 6 /* EM_486 */)) ||
(hdr->e_phentsize != sizeof(Elf32_Phdr)) ||
(hdr->e_phnum < 1) ||
(hdr->e_phentsize*hdr->e_phnum > 65536) )
return -ENOEXEC;
return 0;
}
bool mag3110_magnitude(int fd, int16_t *x, int16_t *y, int16_t *z) {
uint8_t lm[2];
if (read_at(fd, MAG3110_ADDR_OFF_X_LSB, lm, 1) < 0)
return false;
if (read_at(fd, MAG3110_ADDR_OFF_X_MSB, lm + 1, 1) < 0)
return false;
*x = (int16_t) ((lm[0] << 8) | lm[1]);
if (read_at(fd, MAG3110_ADDR_OFF_Y_LSB, lm, 1) < 0)
return false;
if (read_at(fd, MAG3110_ADDR_OFF_Y_MSB, lm + 1, 1) < 0)
return false;
*y = (int16_t) ((lm[0] << 8) | lm[1]);
if (read_at(fd, MAG3110_ADDR_OFF_Z_LSB, lm, 1) < 0)
return false;
if (read_at(fd, MAG3110_ADDR_OFF_Z_MSB, lm + 1, 1) < 0)
return false;
*z = (int16_t) ((lm[0] << 8) | lm[1]);
return true;
}
inline std::size_t read_at(SyncRandomAccessReadDevice& d,
uint64_t offset, const MutableBufferSequence& buffers,
lslboost::system::error_code& ec)
{
return read_at(d, offset, buffers, transfer_all(), ec);
}
inline std::size_t read_at(SyncRandomAccessReadDevice& d,
uint64_t offset, lslboost::asio::basic_streambuf<Allocator>& b,
lslboost::system::error_code& ec)
{
return read_at(d, offset, b, transfer_all(), ec);
}
satcom_result_code IridiumSBD::read_at_command(int16_t timeout)
{
return read_at(_rx_command_buf, &_rx_command_len, timeout);
}
static int try_load_elf(elf_prog_t *prog, long bailout)
{
int err, has_interp;
char i_filename[PATH_MAX]; /* so much easier */
elf_bin_t *bin=&prog->bin, *interp=&prog->interp;
if ( (err = open_elf(prog->filename, bin)) < 0 )
return err;
Elf32_Phdr phdr_bin_tmp[bin->hdr.e_phnum];
bin->phdr = phdr_bin_tmp; /* point to mmapped region later if any */
if (read_at(bin->fd, bin->hdr.e_phoff, bin->phdr,
sizeof(*bin->phdr)*bin->hdr.e_phnum) !=
(long)sizeof(*bin->phdr)*bin->hdr.e_phnum)
return -EIO;
if ( (err = find_interp_name(bin, i_filename, PATH_MAX)) < 0 )
return err;
has_interp = err;
interp->phdr = NULL;
interp->hdr.e_phnum = 0;
if ( has_interp && (err = open_elf(i_filename, interp)) < 0)
return err;
Elf32_Phdr phdr_interp_tmp[interp->hdr.e_phnum];
if ( has_interp )
{
interp->phdr = phdr_interp_tmp; /* point to mmapped region later */
if (read_at(interp->fd, interp->hdr.e_phoff, interp->phdr,
sizeof(*interp->phdr)*interp->hdr.e_phnum) !=
(long)sizeof(*interp->phdr)*interp->hdr.e_phnum)
return -EIO;
}
/* point of no return */
if (bailout)
return 0;
int stack_prot = get_stack_prot(bin);
err = alloc_user_stack(prog, stack_prot);
if ( mmap_binary(bin, 0) & PG_MASK )
raise(SIGKILL);
if ( has_interp && (mmap_binary(interp, 1) & PG_MASK) )
raise(SIGKILL);
/* set up stack */
bin->phdr = mapped_phdr(bin);
if ( has_interp )
interp->phdr = mapped_phdr(interp);
init_user_stack(prog, stack_prot);
if (err & PG_MASK)
raise(SIGKILL);
if (has_interp)
prog->entry = (void *)(interp->base + interp->hdr.e_entry);
else
prog->entry = (void *)(bin->base + bin->hdr.e_entry);
return 0;
}
satcom_result_code IridiumSBD::read_at_msg(int16_t timeout)
{
return read_at(_rx_msg_buf, &_rx_msg_end_idx, timeout);
}
MachObject* MachObject::instantiateFromFile(const char* path, const char* origPath, int fd, const struct stat* info)
{
macho_header header;
uint32_t ncmds = 0;
void* cmd_base = NULL;
uint32_t loader_bias = 0;
size_t offset = 0;
uint32_t map_size = 0;
macho_segment_command* segments[64];
uint32_t segment_count = 0;
void* actual_map_address = NULL;
uint32_t file_size = 0;
if (gVerboseLog) {
lnk::log("institating a MachObject from file\n"
" path : %s\n"
" fd : %p"
, path, fd);
}
/* Read mach-o header */
memset(&header, 0, sizeof(header));
read_at(fd, &header, sizeof(header), 0);
/* Check sanity */
if (header.magic != MH_MAGIC) {
lnk::halt("instantiateFromFile(%s): invalid magic (%p instead of %p)",
path,
header.magic,
MH_MAGIC);
}
/* Read load commands into a buffer */
cmd_base = malloc(header.sizeofcmds);
read_at(fd, cmd_base, header.sizeofcmds, sizeof(macho_header));
/* Set stuff */
ncmds = header.ncmds;
/* Build array of segment load commands */
for (int i = 0; i < ncmds; i++)
{
struct load_command *lcp =
(struct load_command *)((size_t)cmd_base + offset);
offset += lcp->cmdsize;
if (lcp->cmd == LC_SEGMENT)
{
segments[segment_count] = (macho_segment_command*)lcp;
map_size += segments[segment_count]->vmsize;
segment_count++;
}
}
/* Check segment sanity */
if (segment_count < 1) {
lnk::halt("instantiateFromFile(%s): no segments", path);
}
/* Is this a fixed-load dylib? */
if (segments[0]->vmaddr != 0) {
/* Fixed, so not reserving */
loader_bias = 0;
}
else
{
/* Reserve a continous range */
loader_bias = (uint32_t)lnk::mm::reserve(map_size);
}
/* Map the segments */
for (int i = 0; i < segment_count; i++)
{
macho_segment_command* seg = segments[i];
uint32_t delta = 0;
vm_size_t mm_filesize = round_page(seg->filesize);
vm_size_t mm_vmsize = round_page(seg->vmsize);
actual_map_address =
lnk::mm::wire(fd,
pageAlignPtr((void*)addUintPtr2(seg->vmaddr, loader_bias)),
mm_filesize,
seg->fileoff);
if (gVerboseLog) {
lnk::log("(%s:%s): wired at %p (sz: %p)", path, seg->segname, actual_map_address, mm_filesize);
}
delta = mm_vmsize - mm_filesize;
if (delta && !loader_bias)
{
/*
* We need to zero-fill the memory as this space
* isn't backed up be reserved memory.
*/
void* anon_load =
(void*)(addUintPtr3(seg->vmaddr, loader_bias, mm_filesize));
anon_load = pageAlignPtr(anon_load);
/* map anonymous memory to fill the rest */
actual_map_address =
lnk::mm::wire_anon(anon_load,
delta);
if (gVerboseLog) {
lnk::log("(%s:%s): wired anon at %p (sz: %p)", path, seg->segname, actual_map_address, delta);
}
}
}
/* Add __TEXT to the linker table */
linker_image_table_add((uintptr_t)addUintPtr2(segments[0]->vmaddr, loader_bias),
(size_t)segments[0]->vmsize,
path);
/* Instiate a new object */
MachObject* obj = new MachObject();
/* Set the object properties */
obj->fSlide = (uintptr_t)loader_bias;
obj->fHeader = (macho_header*)addUintPtr2(segments[0]->vmaddr, loader_bias);
obj->fModuleName = NULL;
obj->fFilePath = path;
obj->fOrigFilePath = origPath;
obj->fIsSplitSeg = (loader_bias == 0);
return obj;
}
uint8_t mag3110_status(int fd) {
uint8_t value = 0;
read_at(fd, MAG3110_ADDR_DR_STATUS, &value, sizeof(value));
return value;
}
