diff --git a/Makefile b/Makefile
index 61700eb..fc1e208 100644
--- a/Makefile
+++ b/Makefile
@@ -1,7 +1,10 @@
-#
+CFLAGS=	-O0
+DEBUG_FLAGS=	-g
+
 PROG=	pstack
-SRCS=	elf.c pstack.c thread_db.c
+SRCS=	elf.c pstack.c thread_db.c eh.c
 
+LDADD=	-lcxxrt
 # libthread_db.so calls back into gdb for the proc services.  Make all the
 # global symbols visible.
 LDFLAGS+= -Wl,-E
diff --git a/eh.c b/eh.c
new file mode 100644
index 0000000..58c579d
--- /dev/null
+++ b/eh.c
@@ -0,0 +1,574 @@
+/*
+ * Copyright (c) 2017 Michael Zhilin <mizhka@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ *
+ * $FreeBSD$
+ */
+
+/*
+ * This file provides API to parse .eh_frame and .eh_frame_hdr sections of
+ * binaries to retrieve frames without frame pointer information.
+ */
+#include <sys/types.h>
+
+#include "eh.h"
+#include <dwarf.h>    // for DW_CFA_advance_loc, DW_CFA_advance_loc1, DW_CFA...
+#include <err.h>      // for warnx
+#include <stdio.h>    // for printf, NULL
+#include <stdlib.h>   // for free, malloc
+#include <string.h>   // for strcmp, strlen
+#include "elfinfo.h"  // for ElfObject
+
+static int	ehLogging = EH_PRINT_ALL;
+
+/*
+ * table - sorted table of FDEs
+ * count - amount of FDEs in table
+ * key   - key to search
+ * ret   - found value
+ */
+static int	ehFindFDE(const struct ehframehdr_item	*table, int count,
+		    int32_t key, uint32_t *ret);
+
+static uint64_t	_dwarf_decode_uleb128(uint8_t **dp);
+static int64_t	_dwarf_decode_sleb128(uint8_t **dp);
+static int	_dwarf_frame_convert_inst(uint8_t addr_size,
+		    struct eh_record_info *info,
+		    struct eh_cfa_state *rules,
+		    uint32_t *count);
+static uint64_t	_dwarf_decode_lsb(uint8_t **data, int bytes_to_read);
+
+/* Implementation */
+
+static int
+ehFindFDE(const struct ehframehdr_item	*table, int count, int32_t key, 
+    uint32_t *ret)
+{
+	const int	end = count - 1;
+	int		left, right, temp;
+
+	/* index of last item in table */
+	right = end;
+	left = 0;
+
+	if (ehLogging & EH_PRINT_FDE)
+		printf("ehFindFDE tbl: %p, count: %d, key: %x, start: %x, end: %x\n",
+		    table, count, key, table[0].rel_ip, table[end].rel_ip);
+
+	/* if there is only one element and it matches key */
+	if ((end == 0) && (table[0].rel_ip <= key)) {
+		temp = 0;
+		goto success;
+	}
+
+	/* if there is only one element and it doesn't match key */
+	if (end <= 0)
+		return (-1);
+
+	do {
+		temp = (left + right) >> 1;
+
+		/* small region - check both */
+		if (right - left == 1) {
+			if ((table[left].rel_ip <= key) && 
+			    (table[right].rel_ip > key)) {
+				temp = left;
+				goto success;
+			}
+
+			if ((table[right].rel_ip <= key) && 
+			    (right < end) && 
+			    (table[right + 1].rel_ip > key)) {
+				temp = right;
+				goto success;
+			}
+
+			return (-1);
+		}
+
+		if (temp == left)
+			temp = left + 1;
+		else if (temp == right)
+			temp = right - 1;
+
+		/* if we're on the edge, check key */
+		if (((temp == end) && (table[temp].rel_ip < key)) ||
+		    ((temp == 0) && (table[1].rel_ip > key)))
+			goto success;
+
+		/* edge cases - fail */
+		if ((temp == end) || (temp == 0))
+			return (-1);
+
+		/* next step */
+		if (table[temp].rel_ip <= key) {
+			if (table[temp + 1].rel_ip > key)
+				goto success;
+			left = temp;
+		} else
+			right = temp;
+	} while (1);
+
+success:
+	*ret = table[temp].offset; 
+
+	if (ehLogging & EH_PRINT_FDE)
+		printf("ehFindFDE cnt: %d, key: %d, index: %d, found_key:%d, "
+		    "value: %x\n", count, key, temp, table[temp].rel_ip, *ret);
+	
+	return (0);
+}
+
+int
+ehLookupFrame(const struct ehframehdr *ehframehdr, const char* dataAddress,
+    struct eh_cfa_state	*rules)
+{
+	const struct eh_record	*fde, *cie;
+	struct eh_cie_info	*cie_info;
+	struct eh_fde_info	*fde_info;
+	uint32_t		 fde_offset, cnt;
+	uint8_t			*tmp;
+	int			 err;
+
+	if (ehframehdr == NULL)
+		return (-1);
+
+	if (ehFindFDE(&(ehframehdr->base), ehframehdr->n_fdecnt,
+	    rules->eh_rel_ip, &fde_offset) != 0)
+		return (-2);
+
+	cie_info = NULL;
+	fde_info = NULL;
+
+	cie_info = malloc(sizeof(struct eh_cie_info));
+	fde_info = malloc(sizeof(struct eh_fde_info));
+
+	if(cie_info == NULL || fde_info == NULL)
+		goto clean;
+
+	fde = ((void*)ehframehdr + fde_offset);
+	cie = (void*) &(fde->common.cie_offset) - fde->common.cie_offset;
+
+	/* TODO: Unsupported very long FDE. Probably impossible case */
+	if (fde->common.len == 0xFFFFFFFF)
+		goto clean;
+
+	fde_info->common.base = ((void*)ehframehdr + fde_offset);
+	fde_info->common.len = fde->common.len + sizeof(fde->common.len);
+	fde_info->common.instr = ((void*)&(fde->fields.fde_specific.augmentation_len)) +
+	    sizeof(fde->fields.fde_specific.augmentation_len) +
+	    fde->fields.fde_specific.augmentation_len;
+
+	fde_info->common.instr_len = fde_info->common.len -
+	    (fde_info->common.instr - fde_info->common.base);
+
+	fde_info->pc_begin = (uint32_t)( (void*)&(fde->fields.fde_specific.pc_begin) -
+	    (void*)dataAddress) + fde->fields.fde_specific.pc_begin;
+	fde_info->pc_range = fde->fields.fde_specific.pc_range;
+
+	cie_info->common.base = (uint8_t *)cie;
+	cie_info->common.len = cie->common.len + sizeof(cie->common.len);
+
+	/* plus one byte due to field "version" */
+	cie_info->augmentation = (char *)cie_info->common.base +
+	    sizeof(struct eh_record_common) + sizeof(uint8_t);
+
+	/* TODO: Unsupported case - not zR */
+	if(strcmp(cie_info->augmentation, "zR") != 0){
+		warnx("'zR' CIE is only supported\ncie_info: %s", cie_info->augmentation);
+		goto clean;
+	}
+
+	tmp = (uint8_t*)cie_info->augmentation + strlen(cie_info->augmentation) + 1;
+	cie_info->code_aligment = _dwarf_decode_uleb128(&tmp);
+	cie_info->data_aligment = _dwarf_decode_sleb128(&tmp);
+	cie_info->register_ra = *tmp++;
+	cie_info->common.instr = tmp + *tmp + sizeof(uint8_t);
+	cie_info->common.instr_len = cie_info->common.len -
+	    (cie_info->common.instr - cie_info->common.base);
+
+	if (ehLogging & EH_PRINT_FDE)
+		printf("FDE(%p, %d): (0x%x-0x%x) CIE(%p, %d): ra = %x\n",
+		    fde_info->common.base, fde_info->common.len, fde_info->pc_begin,
+		    fde_info->pc_begin + fde_info->pc_range, cie_info->common.base,
+		    cie_info->common.len, cie_info->register_ra);
+
+	rules->current_ip = fde_info->pc_begin;
+	rules->code_aligment = cie_info->code_aligment;
+	rules->data_aligment = cie_info->data_aligment;
+	rules->fde_offset = fde_offset;
+
+	if(fde->fields.fde_specific.augmentation_len == 0) {
+		err = _dwarf_frame_convert_inst(8, &(cie_info->common), rules, &cnt);
+		if (ehLogging & EH_PRINT_FDE)
+			printf("err: %d, cnt: %u\n", err, cnt);
+		/* FIXME: hackish code: I dunno why, but FDE requires shift of IP and SP */
+		rules->current_ip++;
+		rules->cfaoffset -= rules->data_aligment;
+		err = _dwarf_frame_convert_inst(8, &(fde_info->common), rules, &cnt);
+		if (ehLogging & EH_PRINT_FDE)
+			printf("err: %d, cnt: %u\n", err, cnt);
+
+		if ((ehLogging & EH_PRINT_RULES) != 0)
+			ehPrintRules(rules);
+	}
+
+	return (0);
+clean:
+	if(cie_info != NULL)
+		free(cie_info);
+
+	if(fde_info != NULL)
+		free(fde_info);
+
+	return (-3);
+}
+
+void
+ehPrintRules(struct eh_cfa_state *rules)
+{
+
+	if (rules == NULL)
+		return;
+
+	printf("	0x%x: code aligned on %lu, data aligned on %ld\n"
+	       "	CFA: reg<%d> off<%d>\n",
+	    rules->target_ip, rules->code_aligment, rules->data_aligment, 
+	    rules->cfareg, rules->cfaoffset);
+	for (int i = 0; i < REGCNT; i++)
+		if(rules->reg[i] != 0)
+			printf("	REG[%d]: off<%d>\n", i, rules->reg[i]);
+}
+
+
+int32_t
+ehGetRelativeIP(Elf_Addr ip, struct ElfObject *obj)
+{
+
+	return (ip - obj->baseAddr -
+	    ((void*)obj->ehframeHeader - (void*)obj->fileData + obj->ehframe_phys_to_virt));
+}
+
+/*
+ * Passing of DWARF bytecode
+ * Taken from libdwarf to avoid full parsing of debug segments and adapted.
+ */
+
+typedef struct {
+	uint8_t		 fp_base_op;
+	uint8_t		 fp_extended_op;
+	uint16_t	 fp_register;
+	uint64_t	 fp_offset_or_block_len;
+	//uint8_t		*fp_expr_block;
+	uint64_t	 fp_instr_offset;
+} Dwarf_Frame_Op3;
+
+static int
+_dwarf_frame_convert_inst(uint8_t addr_size, struct eh_record_info *info,
+    struct eh_cfa_state	*rules, uint32_t *count)
+{
+	uint8_t 		*p, *pe;
+	uint8_t 		 high2, low6;
+	uint64_t 		 reg, reg2, uoff, soff, blen;
+
+#define	PRINTF(x, ...)					\
+	do {						\
+		if (ehLogging & EH_PRINT_BYTECODE)	\
+			printf(x, ## __VA_ARGS__ );	\
+	} while (0);
+
+	*count = 0;
+
+	p = info->instr;
+	pe = p + info->instr_len;
+
+	while (p < pe && rules->current_ip <= rules->target_ip) {
+		if (*p == DW_CFA_nop) {
+			p++;
+			PRINTF("	DW_CFA_nop\n");
+			(*count)++;
+			continue;
+		}
+
+		high2 = *p & 0xc0;
+		low6 = *p & 0x3f;
+		p++;
+
+		if (high2 > 0) {
+			switch (high2) {
+			case DW_CFA_advance_loc:
+//				SET_BASE_OP(high2);
+//				SET_OFFSET(low6);
+				rules->current_ip += low6 * rules->code_aligment;
+				PRINTF("	DW_CFA_advance_loc offset<%x>\n",low6);
+				break;
+			case DW_CFA_offset:
+//				SET_BASE_OP(high2);
+//				SET_REGISTER(low6);
+				uoff = _dwarf_decode_uleb128(&p);
+//				SET_OFFSET(uoff);
+				rules->reg[low6] = uoff * rules->data_aligment;
+				PRINTF("	DW_CFA_offset reg<%x> offset<%lx>\n",low6, uoff);
+				break;
+			case DW_CFA_restore:
+//				SET_BASE_OP(high2);
+//				SET_REGISTER(low6);
+				//rules->reg[low6] = 0;
+				// TODO: how to mark unused/restored values? bitmask?
+				warnx("UNIMPLEMENTED: DW_CFA_restore reg<%x>",low6);
+				break;
+			default:
+				return (-1);
+			}
+
+			(*count)++;
+			continue;
+		}
+
+//		SET_EXTENDED_OP(low6);
+//
+		switch (low6) {
+		case DW_CFA_set_loc:
+			uoff = _dwarf_decode_lsb(&p, addr_size);
+//			SET_OFFSET(uoff);
+			rules->current_ip = uoff;
+			PRINTF("	DW_CFA_set_loc uoff<%lx>\n",uoff);
+			break;
+		case DW_CFA_advance_loc1:
+			uoff = _dwarf_decode_lsb(&p, 1);
+//			SET_OFFSET(uoff);
+			rules->current_ip += uoff * rules->code_aligment;
+			PRINTF("	DW_CFA_advance_loc1 uoff<%lx>\n",uoff);
+			break;
+		case DW_CFA_advance_loc2:
+			uoff = _dwarf_decode_lsb(&p, 2);
+//			SET_OFFSET(uoff);
+			rules->current_ip += uoff * rules->code_aligment;
+			PRINTF("	DW_CFA_advance_loc2 uoff<%lx>\n",uoff);
+			break;
+		case DW_CFA_advance_loc4:
+			uoff = _dwarf_decode_lsb(&p, 4);
+//			SET_OFFSET(uoff);
+			rules->current_ip += uoff * rules->code_aligment;
+			PRINTF("	DW_CFA_advance_loc4 uoff<%lx>\n",uoff);
+			break;
+		case DW_CFA_offset_extended:
+			reg = _dwarf_decode_uleb128(&p);
+			uoff = _dwarf_decode_uleb128(&p);
+			rules->reg[reg] = uoff * rules->data_aligment;
+			PRINTF("	DW_CFA_offset_extended reg<%lx> uoff<%lx>\n",reg, uoff);
+			break;
+		case DW_CFA_def_cfa:
+			reg = _dwarf_decode_uleb128(&p);
+			uoff = _dwarf_decode_uleb128(&p);
+			rules->cfareg = reg;
+			rules->cfaoffset = uoff;
+			PRINTF("	DW_CFA_def_cfa reg<%lx> uoff<%lx>\n",reg, uoff);
+			break;
+		case DW_CFA_val_offset:
+			reg = _dwarf_decode_uleb128(&p);
+			uoff = _dwarf_decode_uleb128(&p);
+//			SET_REGISTER(reg);
+//			SET_OFFSET(uoff);
+			rules->reg[reg] = uoff * rules->data_aligment; // TODO: mark VAL_OFFSET
+			PRINTF("	DW_CFA_val_offset reg<%lx> uoff<%lx>\n",reg, uoff);
+			break;
+		case DW_CFA_restore_extended:
+			reg = _dwarf_decode_uleb128(&p);
+//			SET_REGISTER(reg);
+			//add implementation
+			warnx("	UNIMPLEMENTED: DW_CFA_restore_extended reg<%lx>",reg);
+			break;
+		case DW_CFA_undefined:
+			reg = _dwarf_decode_uleb128(&p);
+//			SET_REGISTER(reg);
+			//add implementation
+			warnx("	UNIMPLEMENTED: DW_CFA_undefined reg<%lx>",reg);
+			break;
+		case DW_CFA_same_value:
+			reg = _dwarf_decode_uleb128(&p);
+//			SET_REGISTER(reg);
+			//add implementation
+			warnx("	UNIMPLEMENTED: DW_CFA_same_value reg<%lx>",reg);
+			break;
+		case DW_CFA_def_cfa_register:
+			reg = _dwarf_decode_uleb128(&p);
+//			SET_REGISTER(reg);
+			rules->cfareg = reg;
+			PRINTF("	DW_CFA_def_cfa_register reg<%lx>\n",reg);
+			break;
+		case DW_CFA_register:
+			reg = _dwarf_decode_uleb128(&p);
+			reg2 = _dwarf_decode_uleb128(&p);
+//			SET_REGISTER(reg);
+//			SET_OFFSET(reg2);
+			//add implementation
+			warnx("UNIMPLEMENTED: DW_CFA_register reg<%lx> reg2<%lx>\n",reg, reg2);
+			break;
+		case DW_CFA_remember_state:
+			warnx("UNIMPLEMENTED: DW_CFA_remember_state\n");
+			break;
+			//add implementation
+		case DW_CFA_restore_state:
+			warnx("UNIMPLEMENTED: DW_CFA_restore_state\n");
+			break;
+			//add implementation
+		case DW_CFA_def_cfa_offset:
+			uoff = _dwarf_decode_uleb128(&p);
+//			SET_OFFSET(uoff);
+			rules->cfaoffset = uoff;
+			PRINTF("	DW_CFA_def_cfa_offset uoff<%lx>\n",uoff);
+			break;
+		case DW_CFA_def_cfa_expression:
+			blen = _dwarf_decode_uleb128(&p);
+//			SET_BLOCK_LEN(blen);
+			//add implementation
+			warnx("UNIMPLEMENTED: DW_CFA_def_cfa_expression blen<%lx>\n",blen);
+			//SET_EXPR_BLOCK(p, blen);
+			p += blen;
+			break;
+		case DW_CFA_expression:
+		case DW_CFA_val_expression:
+			reg = _dwarf_decode_uleb128(&p);
+			blen = _dwarf_decode_uleb128(&p);
+//			SET_REGISTER(reg);
+//			SET_BLOCK_LEN(blen);
+			//SET_EXPR_BLOCK(p, blen);
+			//add implementation
+			warnx("UNIMPLEMENTED: DW_CFA_expression/DW_CFA_val_expression reg<%lx> blen<%lx>\n",reg, blen);
+			p += blen;
+			break;
+		case DW_CFA_offset_extended_sf:
+			reg = _dwarf_decode_uleb128(&p);
+			soff = _dwarf_decode_sleb128(&p);
+			PRINTF("	DW_CFA_offset_extended_sf reg<%lx> soff<%lx>\n",reg, soff);
+			rules->reg[reg] = soff * rules->data_aligment;
+			break;
+		case DW_CFA_def_cfa_sf:
+			reg = _dwarf_decode_uleb128(&p);
+			soff = _dwarf_decode_sleb128(&p);
+			PRINTF("	DW_CFA_def_cfa_sf reg<%lx> soff<%lx>\n", reg, soff);
+			rules->cfareg = reg;
+			rules->cfaoffset = soff * rules->data_aligment;
+			break;
+		case DW_CFA_val_offset_sf:
+			reg = _dwarf_decode_uleb128(&p);
+			soff = _dwarf_decode_sleb128(&p);
+			PRINTF("	DW_CFA_val_offset_sf reg<%lx> soff<%lx>\n", reg, soff);
+			rules->reg[reg] = soff * rules->data_aligment; //TODO: mark VAL_OFFSET
+			break;
+		case DW_CFA_def_cfa_offset_sf:
+			soff = _dwarf_decode_sleb128(&p);
+//			SET_OFFSET(soff);
+			rules->cfaoffset = soff * rules->data_aligment;
+			PRINTF("	DW_CFA_def_cfa_offset_sf soff<%lx>\n",soff);
+			break;
+		default:
+			return (-1);
+		}
+
+		(*count)++;
+	}
+
+	return (0);
+}
+
+static int64_t
+_dwarf_decode_sleb128(uint8_t **dp)
+{
+	int64_t		 ret;
+	uint8_t		*src, b;
+	int		 shift;
+
+	src = *dp;
+	ret = 0;
+	shift = 0;
+
+	do {
+		b = *src++;
+		ret |= ((b & 0x7f) << shift);
+		shift += 7;
+	} while ((b & 0x80) != 0);
+
+	if ((shift < 64) && ((b & 0x40) != 0))
+		ret |= (-1 << shift);
+
+	*dp = src;
+
+	return (ret);
+}
+
+static uint64_t
+_dwarf_decode_uleb128(uint8_t **dp)
+{
+	uint64_t	 ret;
+	uint8_t		*src, b;
+	int 		 shift;
+	
+	src = *dp;
+	ret = 0;
+	shift = 0;
+
+	do {
+		b = *src++;
+		ret |= ((b & 0x7f) << shift);
+		shift += 7;
+	} while ((b & 0x80) != 0);
+
+	*dp = src;
+
+	return (ret);
+}
+
+static uint64_t
+_dwarf_decode_lsb(uint8_t **data, int bytes_to_read)
+{
+	uint64_t	 ret;
+	uint8_t		*src;
+
+	src = *data;
+	ret = 0;
+
+	switch (bytes_to_read) {
+	case 8:
+		ret |= ((uint64_t) src[4]) << 32 | ((uint64_t) src[5]) << 40;
+		ret |= ((uint64_t) src[6]) << 48 | ((uint64_t) src[7]) << 56;
+		/* no break */
+	case 4:
+		ret |= ((uint64_t) src[2]) << 16 | ((uint64_t) src[3]) << 24;
+		/* no break */
+	case 2:
+		ret |= ((uint64_t) src[1]) << 8;
+		/* no break */
+	case 1:
+		ret |= src[0];
+		break;
+	default:
+		return (0);
+	}
+
+	*data += bytes_to_read;
+
+	return (ret);
+}
+
diff --git a/eh.h b/eh.h
new file mode 100644
index 0000000..f8845bb
--- /dev/null
+++ b/eh.h
@@ -0,0 +1,109 @@
+/*
+ * eh.h
+ *
+ *  Created on: Jul 30, 2017
+ *      Author: mizhka
+ */
+
+#ifndef EH_H_
+#define EH_H_
+
+#include <sys/_stdint.h>  // for uint32_t, int32_t, uint8_t, int64_t, uint64_t
+#include <x86/elf.h>      // for Elf_Addr
+
+#define	REGCNT	200
+
+/* Logging levels */
+
+enum {
+        EH_PRINT_NONE = 0,
+        EH_PRINT_RULES = 1,
+        EH_PRINT_BYTECODE = 2,
+        EH_PRINT_FDE = 4,
+        EH_PRINT_ALL = 255
+};
+
+/* Structures */
+
+struct ElfObject;
+
+struct ehframehdr_item {
+	int32_t		rel_ip;
+	uint32_t	offset;
+};
+
+
+/*
+ * This is combination of
+ *   version (uint8)            structure version (=1)
+ *   eh_frame_ptr_enc (uint8)   encoding of eh_frame_ptr
+ *   fde_count_enc (uint8)      encoding of fde_count
+ *   table_enc (uint8)          encoding of table entries
+ */
+#define	EH_FRAME_MAGIC	0x3b031b01
+
+struct ehframehdr {
+	uint32_t		magic;
+	uint32_t		n_ptr; // pointer to eh_frame section
+	uint32_t		n_fdecnt;
+	struct ehframehdr_item	base;
+};
+
+struct eh_record_fde {
+	int32_t		pc_begin;
+	uint32_t	pc_range;
+	uint8_t		augmentation_len;
+};
+
+struct eh_record_common {
+	uint32_t	len;
+	int32_t		cie_offset;
+};
+
+struct eh_record {
+	struct eh_record_common		common;
+	union {
+		struct eh_record_fde	fde_specific;
+	} fields;
+};
+
+struct eh_record_info {
+	uint8_t		*base;
+	uint8_t		*instr;
+	uint32_t	 len;
+	uint32_t	 instr_len;
+};
+
+struct eh_fde_info {
+	struct eh_record_info	common;
+	int32_t			pc_begin;
+	uint32_t		pc_range;
+
+};
+
+struct eh_cie_info {
+	struct eh_record_info	common;
+	char*			augmentation;
+	uint64_t		code_aligment;
+	int64_t			data_aligment;
+	uint8_t			register_ra;
+};
+
+struct eh_cfa_state {
+	uint32_t	current_ip, target_ip;
+	uint32_t	fde_offset;
+	int32_t		eh_rel_ip;
+	uint32_t	cfareg;
+	uint32_t	cfaoffset;
+	uint64_t	code_aligment;
+	int64_t		data_aligment;
+	int32_t		reg[REGCNT];
+};
+
+
+int	ehLookupFrame(const struct ehframehdr *ehframehdr, 
+	    const char *dataAddress, struct eh_cfa_state *rules);
+int32_t ehGetRelativeIP(Elf_Addr ip, struct ElfObject *obj);
+void	ehPrintRules(struct eh_cfa_state *rules);
+
+#endif /* EH_H_ */
diff --git a/elf.c b/elf.c
index 2da5dd1..0c1d763 100644
--- a/elf.c
+++ b/elf.c
@@ -48,6 +48,7 @@
 #include <unistd.h>
 
 #include "elfinfo.h"
+#include "eh.h"
 
 static unsigned long	elf_hash(const char *name);
 
@@ -59,34 +60,41 @@ static unsigned long	elf_hash(const char *name);
 int
 elfLoadObject(const char *fileName, struct ElfObject **objp)
 {
-	int file, i;
-	const char *p;
-	struct ElfObject *obj;
-	struct stat sb;
-	const Elf_Ehdr *eHdr;
-	const Elf_Shdr **sHdrs, *shdr;
-	const Elf_Phdr **pHdrs;
-	char *data;
+	int			  file, i;
+	const char		 *p;
+	struct ElfObject	 *obj;
+	struct stat		  sb;
+	const Elf_Ehdr		 *eHdr;
+	const Elf_Shdr		**sHdrs, *shdr;
+	const Elf_Phdr		**pHdrs;
+	const struct ehframehdr	 *ehFrameHdr;
+	char			 *data;
 
 	if ((file = open(fileName, O_RDONLY)) == -1) {
 		warn("unable to open executable '%s'", fileName);
 		return (-1);
 	}
+
 	if (fstat(file, &sb) == -1) {
 		close(file);
 		warn("unable to stat executable '%s'", fileName);
 		return (-1);
 	}
+	
 	data = mmap(0, sb.st_size, PROT_READ, MAP_SHARED, file, 0);
 	close(file);
+	
 	if (data == MAP_FAILED) {
 		warn("unable to map executable '%s'", fileName);
 		return (-1);
 	}
+	
 	obj = calloc(1, sizeof(*obj));
 	obj->fileSize = sb.st_size;
 	obj->fileData = data;
 	obj->elfHeader = eHdr = (const Elf_Ehdr *)data;
+	obj->ehframeHeader = NULL;
+	
 	/* Validate the ELF header */
 	if (!IS_ELF(*obj->elfHeader) ||
 	    eHdr->e_ident[EI_CLASS] != ELF_TARG_CLASS ||
@@ -96,8 +104,11 @@ elfLoadObject(const char *fileName, struct ElfObject **objp)
 		munmap(data, sb.st_size);
 		return (-1);
 	}
+
 	obj->programHeaders = pHdrs =
 	    malloc(sizeof(Elf_Phdr *) * (eHdr->e_phnum + 1));
+	obj->baseAddr = 0;
+	
 	for (p = data + eHdr->e_phoff, i = 0; i < eHdr->e_phnum; i++) {
 		pHdrs[i] = (const Elf_Phdr *)p;
 		switch (pHdrs[i]->p_type) {
@@ -107,9 +118,17 @@ elfLoadObject(const char *fileName, struct ElfObject **objp)
 		case PT_DYNAMIC:
 			obj->dynamic = pHdrs[i];
 			break;
+		/*
+		 * Check program headers for load address. If it's ZERO, then
+		 * kernel will load object in ET_DYN_LOAD_ADDRESS
+		 */
+		case PT_LOAD:
+			if (pHdrs[i]->p_paddr == 0 && pHdrs[i]->p_vaddr == 0)
+				obj->baseAddr = ET_DYN_LOAD_ADDR;
 		}
 		p += eHdr->e_phentsize;
 	}
+	
 	pHdrs[i] = 0;
 	obj->sectionHeaders = sHdrs =
 	    malloc(sizeof(Elf_Shdr *) * (eHdr->e_shnum + 1));
@@ -117,6 +136,7 @@ elfLoadObject(const char *fileName, struct ElfObject **objp)
 		sHdrs[i] = (const Elf_Shdr *)p;
 		p += eHdr->e_shentsize;
 	}
+	
 	sHdrs[i] = 0;
 	obj->sectionStrings = eHdr->e_shstrndx != SHN_UNDEF ?
 	    data + sHdrs[eHdr->e_shstrndx]->sh_offset : 0;
@@ -133,9 +153,28 @@ elfLoadObject(const char *fileName, struct ElfObject **objp)
 		else
 			obj->stabStrings = 0;
 	} else {
-	    obj->stabs = 0;
-	    obj->stabCount = 0;
+		obj->stabs = 0;
+		obj->stabCount = 0;
 	}
+
+	if (elfFindSectionByName(obj, ".eh_frame_hdr", &shdr) != -1) {
+		obj->ehframeHeader = ehFrameHdr = (struct ehframehdr*)
+		   (obj->fileData + shdr->sh_offset);
+		obj->ehframe_phys_to_virt= shdr->sh_addr - shdr->sh_offset;
+		if (ehFrameHdr->magic != EH_FRAME_MAGIC) {
+			warnx("Untypical case of eh_frame_hdr, skip parsing");
+			printf("type: %x ptr: %x fdecnt: %x\n", 
+			    ehFrameHdr->magic, ehFrameHdr->n_ptr,
+			    ehFrameHdr->n_fdecnt);
+		}
+	} else if(elfFindSectionByName(obj, ".zdebug_frame", &shdr) != -1) {
+		/*
+		 * Golang doesn't use error handling
+		 */
+		printf("Found compressed DWARF frame section. need support\n");
+
+	}
+
 	return (0);
 }
 
@@ -207,6 +246,7 @@ elfFindSymbolByAddress(struct ElfObject *obj, Elf_Addr addr,
 						*namep = symStrings +
 						    sym->st_name;
 						exact = 1;
+						goto out;
 					}
 				} else {
 					if ((*symp) == 0 || (*symp)->st_value <
@@ -219,6 +259,7 @@ elfFindSymbolByAddress(struct ElfObject *obj, Elf_Addr addr,
 			}
 		}
 	}
+out:
 	return (*symp ? 0 : -1);
 }
 
diff --git a/elfinfo.h b/elfinfo.h
index 1cc90c7..bffcbfc 100644
--- a/elfinfo.h
+++ b/elfinfo.h
@@ -49,6 +49,8 @@ struct ElfObject {
 	const struct stab *stabs;
 	const char 	*stabStrings;
 	int		 stabCount;
+	const struct ehframehdr	*ehframeHeader;
+	uint32_t	ehframe_phys_to_virt;
 };
 
 struct stab {
diff --git a/pstack.1 b/pstack.1
index a423df1..bc4952e 100644
--- a/pstack.1
+++ b/pstack.1
@@ -12,9 +12,11 @@
 .Op Fl a Ar arg-count
 .Op Fl e Ar executable-file
 .Op Fl f Ar frame-count
+.Op Fl n Ar iteration-count
 .Op Fl o
 .Op Fl O
 .Op Fl t
+.Op Fl T Ar thread-id
 .Op Fl v
 .Ar pid | core
 .Nm
@@ -43,9 +45,17 @@ to print a dump of the ELF information in an object file,
 and exit.
 .It Fl e Ar file
 Specify an alternate executable to use for locating symbols in the
-process.  This is useful if the process was started from a stripped
+process.
+This is useful if the process was started from a stripped
 version of an executable, and you have the unstripped version
-available. If examining a corefile, this argument is required.
+available.
+If examining a corefile, this argument is required.
+.It Fl n Ar iteration-count
+Causes
+.Nm
+to display stack traces
+.Ar iteration-count
+times
 .It Fl o
 For each stack frame, display the name of the object in which the current
 function lies
@@ -55,8 +65,11 @@ the current function lies
 .It Fl t
 Display the amount of time that the process was suspended by
 .Nm
+.It Fl T Ar thread-id
+Display stack frame only for thread with specified thread ID
 .It Fl v
-verbose. Display debugging and diagnostics.
+verbose.
+Display debugging and diagnostics.
 .El
 .Sh SEE ALSO
 .Xr ptrace 2
@@ -64,6 +77,6 @@ verbose. Display debugging and diagnostics.
 .Nm
 works only for x86 32/64-bit ELF executables at the moment. (ie, no
 a.out support, and no Alpha support).
-It is also very dependent on the current FreeBSD threads implementation. 
+It is also very dependent on the current FreeBSD threads implementation.
 .Sh AUTHORS
 Peter Edwards <pmedwards@eircom.net>
diff --git a/pstack.c b/pstack.c
index 07cf120..e8506f6 100644
--- a/pstack.c
+++ b/pstack.c
@@ -62,6 +62,7 @@
 #include <unistd.h>
 
 #include "elfinfo.h"
+#include "eh.h"
 #include "pstack.h"
 
 /*
@@ -72,9 +73,13 @@ static int gMaxFrames = 1024;		/* max number of frames to read */
 static int gDoTiming = 0;		/* Report time process was stopped */
 static int gShowObjectNames = 0;	/* show names of objects for each IP */
 static int gVerbose = 0;
+int gThreadID = -1;			/* filter by thread, -1 - all */
+static int gIterations = 1;
 
 /* Amount of time process was suspended (if gDoTiming == 1) */
 static struct timeval gSuspendTime;
+static struct timeval gSuspendLoadedTime;
+
 
 static struct thread_ops *thread_ops[] = {
 	&thread_db_ops,
@@ -88,14 +93,17 @@ static int	procOpen(pid_t pid, const char *exeName,
 static int	procFindObject(struct Process *proc, Elf_Addr addr,
 			struct ElfObject **objp);
 static int	procDumpStacks(FILE *file, struct Process *proc, int indent);
-static void	procAddElfObject(struct Process *proc,
-			struct ElfObject *obj, Elf_Addr base);
+static void	procDumpThreadStacks(FILE *file, struct Process *proc,
+		    struct Thread *thread, int indent);
+static void	procAddElfObject(struct Process *proc, struct ElfObject *obj,
+		    Elf_Addr base);
 static void	procFree(struct Process *proc);
 static void	procFreeThreads(struct Process *proc);
 static void	procFreeObjects(struct Process *proc);
 static void	procLoadSharedObjects(struct Process *proc);
 static int	procGetRegs(struct Process *proc, struct reg *reg);
 static int	procSetupMem(struct Process *proc, pid_t pid, const char *core);
+
 static int	usage(void);
 
 int
@@ -109,7 +117,7 @@ main(int argc, char **argv)
 	struct ElfObject *dumpObj;
 	struct thread_ops **tdops;
 
-	while ((c = getopt(argc, argv, "a:d:e:f:hoOs:tv")) != -1) {
+	while ((c = getopt(argc, argv, "a:d:e:f:n:T:hoOs:tv")) != -1) {
 		switch (c) {
 		case 'a':
 			gFrameArgs = atoi(optarg);
@@ -134,6 +142,9 @@ main(int argc, char **argv)
 		case 'h':
 			usage();
 			return 0;
+		case 'n':
+			gIterations = MAX(1, atoi(optarg));
+			break;
 		case 'o':
 			gShowObjectNames = 1;
 			break;
@@ -146,6 +157,9 @@ main(int argc, char **argv)
 		case 't':
 			gDoTiming = 1;
 			break;
+		case 'T':
+			gThreadID = atoi(optarg);
+			break;
 		case 'v':
 			gVerbose++;
 			gShowObjectNames++;
@@ -161,27 +175,38 @@ main(int argc, char **argv)
 		(*tdops)->startup();
 		tdops++;
 	}
-	for (err = 0, i = optind; i < argc; i++) {
-		pid = atoi(argv[i]);
-		if (pid == 0 || (kill(pid, 0) == -1 && errno == ESRCH)) {
-			/* Assume argv[i] is a core file */
-			coreFile = argv[i];
-			pid = -1;
-		} else {
-			/* Assume argv[i] is a pid */
-			coreFile = 0;
-		}
-		if (procOpen(pid, execFile, coreFile, &proc) == 0) {
-			procDumpStacks(stdout, proc, 0);
-			procFree(proc);
-			if (gDoTiming)
-				fprintf(stderr,
-				    "suspended for %zd.%06ld secs\n",
-				    gSuspendTime.tv_sec, gSuspendTime.tv_usec);
-		} else {
-			err = EX_OSERR;
+	for (int iteration = 0; iteration < gIterations; iteration++) {
+		if (iteration > 0)
+			usleep(200000);
+
+		for (err = 0, i = optind; i < argc; i++) {
+			pid = atoi(argv[i]);
+			if (pid == 0 || (kill(pid, 0) == -1 && errno == ESRCH)) {
+				/* Assume argv[i] is a core file */
+				coreFile = argv[i];
+				pid = -1;
+			} else {
+				/* Assume argv[i] is a pid */
+				coreFile = 0;
+			}
+
+			if (procOpen(pid, execFile, coreFile, &proc) == 0) {
+				procDumpStacks(stdout, proc, 0);
+				procFree(proc);
+				if (gDoTiming) {
+					fprintf(stderr,
+						"suspended for %zd.%06ld secs\n",
+						gSuspendTime.tv_sec, gSuspendTime.tv_usec);
+					fprintf(stderr,
+						"loaded in %zd.%06ld secs\n",
+						gSuspendLoadedTime.tv_sec, gSuspendLoadedTime.tv_usec);
+				}
+			} else {
+				err = EX_OSERR;
+			}
 		}
 	}
+
 	return (err);
 }
 
@@ -207,7 +232,7 @@ procOpen(pid_t pid, const char *exeName, const char *coreFile,
 	struct Process **procp)
 {
 	struct Thread *t;
-	struct timeval start, end;
+	struct timeval start, loadedObjects, end;
 	int i, status, rc;
 	char tmpBuf[PATH_MAX];
 	struct Process *proc;
@@ -300,6 +325,8 @@ procOpen(pid_t pid, const char *exeName, const char *coreFile,
 	}
 	/* Attach any dynamically-linked libraries */
 	procLoadSharedObjects(proc);
+	if (gDoTiming)
+		gettimeofday(&loadedObjects, 0);
 
 	/* See if we have any threads. */
 	tdops = thread_ops;
@@ -318,9 +345,9 @@ procOpen(pid_t pid, const char *exeName, const char *coreFile,
 	procGetRegs(proc, &regs);
 	/* Trace the active thread */
 #ifdef __LP64__
-	if ((t = procReadThread(proc, regs.r_rbp, regs.r_rip)) != NULL) {
+	if ((t = procReadThread(proc, regs.r_rbp, regs.r_rip, regs.r_rsp)) != NULL) {
 #else
-	if ((t = procReadThread(proc, regs.r_ebp, regs.r_eip)) != NULL) {
+	if ((t = procReadThread(proc, regs.r_ebp, regs.r_eip, regs.r_esp)) != NULL) {
 #endif
 		t->id = -1;
 		t->running = 1;
@@ -341,6 +368,12 @@ procOpen(pid_t pid, const char *exeName, const char *coreFile,
 				gSuspendTime.tv_sec -= 1;
 				gSuspendTime.tv_usec += 1000000;
 			}
+			gSuspendLoadedTime.tv_sec = loadedObjects.tv_sec - start.tv_sec;
+			gSuspendLoadedTime.tv_usec = loadedObjects.tv_usec - start.tv_usec;
+			if (gSuspendLoadedTime.tv_usec < 0) {
+				gSuspendLoadedTime.tv_sec -= 1;
+				gSuspendLoadedTime.tv_usec += 1000000;
+			}
 		}
 	}
 	/* Success */
@@ -376,7 +409,7 @@ size_t
 procReadMem(struct Process *proc, void *ptr, Elf_Addr remoteAddr, size_t size)
 {
 	struct ptrace_io_desc pio;
-	int rc;
+	int rc, err;
 	size_t fragSize, readLen;
 	const Elf_Phdr **hdr;
 	const char *data;
@@ -418,8 +451,12 @@ procReadMem(struct Process *proc, void *ptr, Elf_Addr remoteAddr, size_t size)
 				pio.piod_offs = (void *)pageLoc;
 				pio.piod_addr = p;
 				pio.piod_len = pagesize;
-				if (ptrace(PT_IO, proc->pid, (caddr_t)&pio, 0) < 0 ||
-				    pio.piod_len != pagesize) {
+				errno = 0;
+				err = ptrace(PT_IO, proc->pid, (caddr_t)&pio, 0);
+				if (err < 0 || pio.piod_len != pagesize) {
+					if (gVerbose)
+						warnx("ptrace_read err(%d): %s for address 0x%lx",
+						    err, strerror(errno), pageLoc);
 					free(p);
 					return (readLen);
 				}
@@ -468,42 +505,268 @@ procReadMem(struct Process *proc, void *ptr, Elf_Addr remoteAddr, size_t size)
  * thread structure to the "threadList" of the process.
  */
 struct Thread *
-procReadThread(struct Process *proc, Elf_Addr bp, Elf_Addr ip)
+procReadThread(struct Process *proc, Elf_Addr bp, Elf_Addr ip, Elf_Addr sp)
 {
-	int frameCount, i;
-	struct StackFrame *frame;
+	int		err, frameCount, i, pos;
+	int32_t		ip_offset, rel_ip;
+
+	Elf_Addr	next_ip, next_bp, next_sp;
+
+	struct eh_cfa_state	*rules;
+	struct ElfObject	*objp;
+	struct StackFrame	*frame;
+	struct Thread		*thread;
+
 	const int frameSize = sizeof(*frame) + sizeof(Elf_Word) * gFrameArgs;
-	struct Thread *thread;
+
+	/* Initialize next registers by initial values, prepare for shifting */
+	next_ip = ip;
+	next_bp = bp;
+	next_sp = sp;
 
 	/* Check to see if we have already seen this thread. */
 	for (thread = proc->threadList; thread != NULL; thread = thread->next) {
 		frame = STAILQ_FIRST(&thread->stack);
-		if (frame->ip == ip && frame->bp == bp)
+		if (frame->ip == ip && frame->sp == sp)
 			return (thread);
 	}
-	
+
+	if(gVerbose > 0)
+		fprintf( stderr,
+		    "\n---- thread:\tip = 0x%016lx bp = 0x%016lx sp = 0x%016lx\n",
+		    ip, bp, sp);
+
+	/*
+	 * There are 2 options:
+	 * 	 - initial frame is frame pointer optimized (FPO)
+	 * 	 - initial frame is with frame pointer, not optimized
+	 *
+	 * If FPO, then if there is EH in object file, we can find CFA and
+	 * virtual BP.
+	 * If there is no EH, assume no frame pointer optimization.
+	 */
+//	if (procFindObject(proc, ip, &objp) == 0) {
+//		rules = malloc(sizeof(struct eh_cfa_state));
+//		if(rules == NULL) {
+//			abort();
+//		}
+//		memset(rules, 0, sizeof(struct eh_cfa_state));
+//		rules->target_ip = ip - objp->baseAddr;
+//		rules->eh_rel_ip = ehGetRelativeIP(ip, objp);
+//
+//		err = ehLookupFrame(objp->ehframeHeader, objp->fileData, rules);
+//
+//		if (err != 0) {
+//			warnx("Can't read eh segments");
+//			abort();
+//		}
+//
+//		if (gVerbose > 1)
+//			ehPrintRules(rules);
+//
+//		if (rules->cfareg == 7)
+//		{
+//			next_sp = sp + rules->cfaoffset;
+//		}
+//
+//		next_ip = rules->reg[16]
+//		// + (2 * rules->data_aligment)
+//
+//
+//		free(rules);
+//	} else {
+//		/* TODO: ??? */
+//		warnx("jitted code: ip = 0x%lx sp = 0x%lx bp = 0x%lx", ip, sp, bp);
+//	}
+
 	thread = malloc(sizeof(struct Thread));
 	thread->running = 0;
+	ip_offset = sizeof(Elf_Addr);
 	STAILQ_INIT(&thread->stack);
-	/* Put a bound on the number of iterations. */
+
+	/*
+	 * Iterate over frames
+	 * Put a bound on the number of iterations.
+	 */
 	for (frameCount = 0; frameCount < gMaxFrames; frameCount++) {
+		/*
+		 * Allocate memory for new frame, fill it by registers and store this
+		 * frame with args in the Thread
+		 */
 		frame = malloc(frameSize);
-		/* Store this frame, and its args in the Thread */
+		if (frame == NULL) {
+			//TODO: error handling
+		}
 		frame->ip = ip;
 		frame->bp = bp;
+		frame->sp = sp;
+		frame->broken = 0;
 		STAILQ_INSERT_TAIL(&thread->stack, frame, link);
+
+		if(gVerbose > 1)
+			warnx("frame#%d:\tip = 0x%016lx bp = 0x%016lx sp = 0x%016lx offset = 0x%d",
+				frameCount, ip, bp, sp, ip_offset);
+
+		/* XXX: it's broken. Attempt to fetch arguments */
 		for (i = 0; i < gFrameArgs; i++)
 			if (procReadMem(proc, &frame->args[i],
 			    bp + sizeof(Elf_Word) * 2 + i * sizeof(Elf_Word),
 			    sizeof(Elf_Word)) != sizeof(Elf_Word))
 				break;
 		frame->argCount = i;
-		/* Read the next frame */
-		if (procReadMem(proc, &ip, bp + sizeof(bp), sizeof(ip))
-		    != sizeof(ip) || ip == 0 ||
-		    procReadMem(proc, &bp, bp, sizeof(bp)) != sizeof(bp) ||
-		    bp <= frame->bp)
+
+		if (procFindObject(proc, ip, &objp) == 0) {
+			/*
+			 * Let's suppose FPO optimization. Try to find object
+			 * of last known frame and look for eh_frame_hdr info.
+			 */
+			rules = malloc(sizeof(struct eh_cfa_state));
+			if(rules == NULL) {
+				abort();
+			}
+			memset(rules, 0, sizeof(struct eh_cfa_state));
+			rules->target_ip = ip - objp->baseAddr;
+			rules->eh_rel_ip = ehGetRelativeIP(ip, objp);
+
+			if (gVerbose > 2)
+				warnx("ehLookup:\tof = 0x%016x bp = 0x%016lx"
+				    "\n\t\t(file %s at %p)"
+				    "\n\t\t(EH eh_rel_ip: %d, ehframeHeader: %p)",
+				    rules->target_ip, frame->bp,
+				    objp->fileName, objp->fileData,
+				    rules->eh_rel_ip, objp->ehframeHeader);
+
+			err = ehLookupFrame(objp->ehframeHeader, objp->fileData, rules);
+
+			if (err != 0) {
+				if (gVerbose > 2)
+					warnx("Can't read eh segments: %d", err);
+				break;
+			}
+
+			if (gVerbose > 1)
+				ehPrintRules(rules);
+
+			if (rules->cfareg == 7)
+			{
+				/* SP register number is 7 */
+				next_sp = sp + rules->cfaoffset;
+				//XXX:
+				if (frameCount == 0) {
+					next_sp -= sizeof(Elf_Addr);
+				}
+				next_ip = next_sp + rules->reg[0x10];
+			} else if (rules->cfareg == 6)
+			{
+				/* BP register number is 6 */
+				next_bp = bp + rules->cfaoffset;
+				next_ip = next_bp + rules->reg[0x10];
+				if (rules->reg[6] != 0) {
+					next_bp += rules->reg[6];
+				}
+			} else {
+				/* if CFA register is neither SP nor BP, then raise error */
+				warnx("CFA is not SP/BP offset:"
+				    "0%x 0x%lx / 0x%x (%s)",
+				    rules->cfareg, ip, rules->target_ip,
+				    objp->fileName);
+				abort();
+			}
+
+			if (rules->reg[6] != 0 &&
+				procReadMem(proc, &next_bp, next_bp, sizeof(Elf_Addr)) != sizeof(Elf_Addr)) {
+				frame->broken = '!';
+				free(rules);
+				break;
+			}
+
+			if (rules->reg[7] != 0 &&
+				procReadMem(proc, &next_sp, next_sp, sizeof(Elf_Addr)) != sizeof(Elf_Addr)) {
+				frame->broken = '!';
+				free(rules);
+				break;
+			}
+
+			free(rules);
+
+			/* Fetch next RIP register value */
+			if (procReadMem(proc, &next_ip, next_ip, sizeof(Elf_Addr)) != sizeof(Elf_Addr)) {
+				frame->broken = '!';
+				break;
+			}
+		} else {
+			/*
+			 * Fetch caller IP and BP registers assuming actual frame contains:
+			 *  - caller ip is *[bp + word]
+			 *  - caller bp is *[bp]
+			 */
+			if (gVerbose > 1)
+				warnx("bad #%d:\tip = 0x%016lx bp = 0x%016lx"
+				    " sp = 0x%016lx prev_ip = 0x%016lx",
+				    frameCount + 1, ip, frame->ip,
+				    frame->sp, frame->bp);
+
+			if (procReadMem(proc, &next_ip, bp + ip_offset, sizeof(Elf_Addr)) != sizeof(Elf_Addr)) {
+				frame->broken = '!';
+				break;
+			}
+
+			if (procReadMem(proc, &next_bp, bp, sizeof(Elf_Addr)) != sizeof(Elf_Addr)) {
+				frame->broken = '?';
+				break;
+			}
+		}
+//		} else {
+//			if (procReadMem(proc, &next_ip, bp, sizeof(Elf_Addr)) != sizeof(Elf_Addr)) {
+//				frame->broken = '!';
+//				break;
+//			}
+//
+//			bp += sizeof(ip);
+//			sp += sizeof(ip);
+//		}
+
+		if (ip == 0) {
+			frame->broken = '.';
+			if(gVerbose > 1)
+				procDumpThreadStacks(stdout, proc, thread, 4);
 			break;
+		}
+
+		/*
+		 * We need more love for this place. If previous frame
+		 * is BP-supplied, so our SP is previous BP + 2 and CFA
+		 * is previous BP + 2 + cfaoffset. :et's imagine that we're
+		 * also BP-supplied, so our frame pointer is CFA-2, i.e.
+		 * previous BP + cfaoffset. Return address is CFA + RA shift,
+		 * i.e. our frame pointer + 2 + RA shift
+		 */
+		//next_bp = frame->bp + rules->cfaoffset;
+		/* TODO: 0x10 is return address, take it from EH information */
+		//ip_offset = rules->reg[0x10] - (2 * rules->data_aligment);
+
+		frame->broken = 0;
+		sp = next_sp;
+		bp = next_bp;
+		ip = next_ip;
+
+		continue;
+fpo_fail:
+		if(rules != NULL)
+			free(rules);
+
+		if ((bp <= frame->bp) || ((bp - frame->bp) > 0x100000)){
+			if (gVerbose > 1)
+				frame->broken = '*';
+			break;
+		}
+
+		ip_offset = sizeof(bp);
+		next_sp = frame->bp + sizeof(ip);
+
+		sp = next_sp;
+		bp = next_bp;
+		ip = next_ip;
 	}
 	thread->next = proc->threadList;
 	proc->threadList = thread;
@@ -534,18 +797,97 @@ procFindObject(struct Process *proc, Elf_Addr addr, struct ElfObject **objp)
 	return (-1);
 }
 
+static void
+procDumpThreadStacks(FILE *file, struct Process *proc, struct Thread *thread, int indent)
+{
+	struct StackFrame	*frame;
+	struct ElfObject	*obj;
+	const Elf_Sym		*sym;
+	const char		*padding, *fileName, *symName, *p;
+	int			 tmp;
+	size_t			 size;
+	char 			*buf, *tmpStr;
+
+	if (gThreadID != -1 && gThreadID != thread->id)
+		return;
+
+	padding = pad(indent);
+	size = 1024 * sizeof(char);
+
+	fprintf(file, "%s----------------- thread %d ",
+	    padding, thread->id);
+	if (thread->running)
+		printf("(running) ");
+	fprintf(file, "-----------------\n");
+	STAILQ_FOREACH(frame, &thread->stack, link) {
+		symName = fileName = "????????";
+		sym = NULL;
+		obj = NULL;
+		buf = NULL;
+		if (procFindObject(proc, frame->ip, &obj) == 0) {
+			fileName = obj->fileName;
+			/* TODO: batch frames for same object */
+			elfFindSymbolByAddress(obj,
+			    frame->ip - obj->baseAddr, STT_FUNC, &sym,
+			    &symName);
+
+			if (symName != NULL && strlen(symName) > 2 &&
+			    symName[0] == '_' && symName[1] == 'Z') {
+				buf = malloc(size);
+				buf = __cxa_demangle(symName, buf, &size, &tmp);
+				if ( tmp != 0 ) {
+					free(buf);
+					buf = NULL;
+				} else {
+					symName = buf;
+				}
+			}
+		}
+		if (gVerbose > 1 && frame->broken != 0)
+			fprintf(file, "%c", frame->broken);
+		fprintf(file, "%s%#*zx ", padding - 1, 11, frame->ip);
+		if (gVerbose) /* Show ebp for verbose */
+		    fprintf(file, "0x%zx ", frame->bp);
+		if (obj && gShowObjectNames) {
+			fprintf(file, "in %s\t",
+			    gShowObjectNames > 1 ||
+			    !(p = strrchr(obj->fileName, '/')) ?
+			    obj->fileName : p + 1);
+		}
+		fprintf(file, "%s", symName);
+		if (buf != NULL) {
+			free (buf);
+			buf = NULL;
+		}
+
+		if (obj && sym != NULL)
+			fprintf(file, " + %zx", frame->ip - obj->baseAddr -
+			    sym->st_value);
+
+#if 0
+		fprintf(file, " (");
+		if (frame->argCount) {
+			for (tmp = 0; tmp < frame->argCount - 1; tmp++)
+				fprintf(file, "%x, ", frame->args[tmp]);
+			fprintf(file, "%x", frame->args[tmp]);
+		}
+		fprintf(file, ")");
+#endif
+
+		fprintf(file, "\n");
+	}
+	fprintf(file, "\n");
+	return;
+}
+
 /*
  * Print a stack trace of each stack in the process
  */
 static int
 procDumpStacks(FILE *file, struct Process *proc, int indent)
 {
-	struct StackFrame *frame;
-	struct ElfObject *obj;
-	int i;
-	struct Thread *thread;
-	const Elf_Sym *sym;
-	const char *fileName, *symName, *p, *padding;
+	struct Thread	*thread;
+	const char	*padding;
 
 	padding = pad(indent);
 	fprintf(file, "%s", padding);
@@ -554,45 +896,9 @@ procDumpStacks(FILE *file, struct Process *proc, int indent)
 	else
 		fprintf(file, "%d", proc->pid);
 	fprintf(file, ": %s\n", proc->execImage->fileName);
-	for (thread = proc->threadList; thread; thread = thread->next) {
-		fprintf(file, "%s----------------- thread %d ",
-		    padding, thread->id);
-		if (thread->running)
-			printf("(running) ");
-		fprintf(file, "-----------------\n");
-		STAILQ_FOREACH(frame, &thread->stack, link) {
-			symName = fileName = "????????";
-			sym = NULL;
-			obj = NULL;
-			if (procFindObject(proc, frame->ip, &obj) == 0) {
-				fileName = obj->fileName;
-				elfFindSymbolByAddress(obj,
-				    frame->ip - obj->baseAddr, STT_FUNC, &sym,
-				    &symName);
-			}
-			fprintf(file, "%s%#*zx ", padding - 1, 11, frame->ip);
-			if (gVerbose) /* Show ebp for verbose */
-			    fprintf(file, "0x%zx ", frame->bp);
-			fprintf(file, "%s (", symName);
-			if (frame->argCount) {
-				for (i = 0; i < frame->argCount - 1; i++)
-					fprintf(file, "%x, ", frame->args[i]);
-				fprintf(file, "%x", frame->args[i]);
-			}
-			fprintf(file, ")");
-			if (obj && sym != NULL)
-				printf(" + %zx", frame->ip - obj->baseAddr -
-				    sym->st_value);
-			if (obj && gShowObjectNames) {
-				printf(" in %s",
-				    gShowObjectNames > 1 ||
-				    !(p = strrchr(obj->fileName, '/')) ?
-				    obj->fileName : p + 1);
-			}
-			printf("\n");
-		}
-		fprintf(file, "\n");
-	}
+	for (thread = proc->threadList; thread; thread = thread->next)
+		procDumpThreadStacks(file, proc, thread, indent);
+
 	return (0);
 }
 
@@ -603,11 +909,13 @@ static void
 procAddElfObject(struct Process *proc, struct ElfObject *obj, Elf_Addr base)
 {
 	obj->next = proc->objectList;
-	obj->baseAddr = base;
+	if (base > 0)
+		obj->baseAddr = base;
+
 	proc->objectList = obj;
 	proc->objectCount++;
 	if (gVerbose)
-		warnx("object loaded: %s @ %zu", obj->fileName, base);
+		warnx("object loaded: %s @ 0x%lx", obj->fileName, obj->baseAddr);
 }
 
 /*
@@ -677,8 +985,9 @@ procLoadSharedObjects(struct Process *proc)
 		lAddr = (Elf_Addr)map.l_addr;
 		if (lAddr <= proc->execImage->elfHeader->e_entry) {
 			if (gVerbose > 1)
-				warnx("skipping \"%s\" as executable image",
-				    path);
+				// it's normal situation
+				warnx("skipping \"%s\" as executable image: %lx %lx",
+				    path, lAddr, proc->execImage->elfHeader->e_entry);
 			continue;
 		}
 		if (proc->abiPrefix && access(prefixedPath, R_OK) == 0)
@@ -711,7 +1020,7 @@ procFindRDebugAddr(struct Process *proc)
 			    obj->dynamic->p_offset + dyn);
 			if (dynp->d_tag == DT_DEBUG &&
 			    procReadMem(proc, &dyno,
-			    obj->dynamic->p_vaddr + dyn, sizeof(dyno)) ==
+			    obj->dynamic->p_vaddr + dyn + obj->baseAddr, sizeof(dyno)) ==
 			    sizeof (dyno))
 				return(dyno.d_un.d_ptr);
 		}
diff --git a/pstack.h b/pstack.h
index 7c1de8f..5382333 100644
--- a/pstack.h
+++ b/pstack.h
@@ -7,6 +7,8 @@ struct StackFrame {
 	STAILQ_ENTRY(StackFrame) link;
 	Elf_Addr	ip;
 	Elf_Addr	bp;
+	Elf_Addr	sp;
+	char		broken;
 	int		argCount;
 	Elf_Word	args[1];
 };
@@ -57,14 +59,21 @@ struct thread_ops {
 };
 
 extern struct thread_ops thread_db_ops;
+extern int gThreadID;
 
 size_t	procReadMem(struct Process *proc, void *ptr, Elf_Addr remoteAddr,
 	    size_t size);
 int	procReadVar(struct Process *proc, struct ElfObject *obj,
 	    const char *name, int *value);
 struct Thread *procReadThread(struct Process *proc, Elf_Addr bp,
-	    Elf_Addr ip);
+	    Elf_Addr ip, Elf_Addr sp);
 size_t	procWriteMem(struct Process *proc, const void *ptr, Elf_Addr remoteAddr,
 	    size_t size);
 
+/* Use C++ ABI to demangle C++ functions */
+char* __cxa_demangle(const char* mangled_name,
+                     char* buf,
+                     size_t* n,
+                     int* status);
+
 #endif
diff --git a/thread_db.c b/thread_db.c
index fc41fe3..66c60d1 100644
--- a/thread_db.c
+++ b/thread_db.c
@@ -159,6 +159,9 @@ find_new_threads_callback(const td_thrhandle_t *th_p, void *data)
 		return (0);
 	}
 
+	if (gThreadID != -1 && gThreadID != ti.ti_tid)
+		return (0);
+
 	/* Ignore zombie */
 	if (ti.ti_state == TD_THR_UNKNOWN || ti.ti_state == TD_THR_ZOMBIE)
 		return (0);
@@ -172,9 +175,9 @@ find_new_threads_callback(const td_thrhandle_t *th_p, void *data)
 
 	proc = data;
 #ifdef __LP64__
-	t = procReadThread(proc, gregset[0].r_rbp, gregset[0].r_rip);
+	t = procReadThread(proc, gregset[0].r_rbp, gregset[0].r_rip, gregset[0].r_rsp);
 #else
-	t = procReadThread(proc, gregset[0].r_ebp, gregset[0].r_eip);
+	t = procReadThread(proc, gregset[0].r_ebp, gregset[0].r_eip, gregset[0].r_esp);
 #endif
 	if (t != NULL) {
 		t->id = ti.ti_tid;