// Update gyro offsets with new values. New offset values are substracted to actual offset values.
// offset values in gyro LSB units (as read from registers)
void MPU6000::set_gyro_offsets(int16_t offsetX, int16_t offsetY, int16_t offsetZ)
{
int16_t aux_int;
if (offsetX != 0){
// Read actual value
aux_int = (register_read(MPUREG_XG_OFFS_USRH)<<8) | register_read(MPUREG_XG_OFFS_USRL);
aux_int -= offsetX<<1; // Adjust to internal units
// Write to MPU registers
register_write(MPUREG_XG_OFFS_USRH, (aux_int>>8)&0xFF);
register_write(MPUREG_XG_OFFS_USRL, aux_int&0xFF);
}
uint64_t register_read_memory(void *opaque, hwaddr addr,
unsigned size)
{
RegisterInfoArray *reg_array = opaque;
RegisterInfo *reg = NULL;
uint64_t read_val;
int i;
for (i = 0; i < reg_array->num_elements; i++) {
if (reg_array->r[i]->access->addr == addr) {
reg = reg_array->r[i];
break;
}
}
if (!reg) {
qemu_log_mask(LOG_GUEST_ERROR, "Read to unimplemented register at " \
"address: %#" PRIx64 "\n", addr);
return 0;
}
read_val = register_read(reg, size * 8, reg_array->prefix,
reg_array->debug);
return extract64(read_val, 0, size * 8);
}
static inline void
print_far(bitstream_parser_t *parser) {
const guint32 far = register_read(parser, FAR);
gchar far_name[32];
snprintf_far(far_name, sizeof(far_name), far);
debit_log(L_BITSTREAM, "FAR is [%08x], %s", far, far_name);
}
void regdump(void){
for(uint16_t reg=0x00; reg<=0x3f; reg++){
register_read(reg);
}
}
uint64_t register_read_memory(void *opaque, hwaddr addr,
unsigned size)
{
RegisterInfoArray *reg_array = opaque;
RegisterInfo *reg = NULL;
uint64_t read_val;
uint64_t re;
int i;
for (i = 0; i < reg_array->num_elements; i++) {
if (reg_array->r[i]->access->addr == addr) {
reg = reg_array->r[i];
break;
}
}
if (!reg) {
qemu_log_mask(LOG_GUEST_ERROR, "%s: read to unimplemented register " \
"at address: %#" PRIx64 "\n", reg_array->prefix, addr);
return 0;
}
/* Generate appropriate read enable mask */
re = register_enabled_mask(reg->data_size, size);
read_val = register_read(reg, re, reg_array->prefix,
reg_array->debug);
return extract64(read_val, 0, size * 8);
}
static inline void
far_increment_mna(bitstream_parser_t *bitstream) {
const id_vlx_t chiptype = bitstream->type;
sw_far_t far;
fill_swfar(&far, register_read(bitstream, FAR));
_far_increment_mna(chiptype, &far);
register_write(bitstream, FAR, get_hwfar(&far));
}
static gint
handle_fdri_write(bitstream_parsed_t *parsed,
bitstream_parser_t *parser,
const unsigned length) {
bytearray_t *ba = &parser->ba;
const gchar *frame = bytearray_get_ptr(ba);
guint i, nrframes;
guint32 last_far = 0;
/* Frame length writes must be a multiple of the flr length */
if (length % frame_length) {
debit_log(L_BITSTREAM,"%i bytes in FDRI write, "
"which is inconsistent with the FLR value %i",
length, frame_length);
return -1;
}
nrframes = length / frame_length;
/* We handle here a complete series of writes, so that we have
the ability to see the start and end frames */
last_far = register_read(parser, FAR);
for (i = 0; i < nrframes; i++) {
/* The first write of a FDRI write in WCFG mode does not flush the
previous writes. As I don't know what other modes may be on, be
conservative wrt to mode setting */
if (i != 0)
/* flush the previous frame into the frame array with the previous
FAR address */
record_frame(parsed, parser, last_far);
last_far = register_read(parser, FAR);
parser->last_frame = frame;
far_increment(parser);
frame += frame_length * sizeof(guint32);
}
debit_log(L_BITSTREAM,"%i frames written to fdri", i);
return length;
}
void MPU6000::hardware_init()
{
// MPU6000 chip select setup
pinMode(_cs_pin, OUTPUT);
digitalWrite(_cs_pin, HIGH);
delay(1);
// Chip reset
register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_DEVICE_RESET);
delay(100);
// Wake up device and select GyroZ clock (better performance)
register_write(MPUREG_PWR_MGMT_1, BIT_PWR_MGMT_1_CLK_ZGYRO);
delay(1);
register_write(MPUREG_PWR_MGMT_2, 0x00); // only used for wake-up in accelerometer only low power mode
delay(1);
// Disable I2C bus (recommended on datasheet)
register_write(MPUREG_USER_CTRL, BIT_USER_CTRL_I2C_IF_DIS);
delay(1);
// SAMPLE RATE
register_write(MPUREG_SMPLRT_DIV, MPUREG_SMPLRT_200HZ); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
delay(1);
// FS & DLPF FS=2000º/s, DLPF = 98Hz (low pass filter)
register_write(MPUREG_CONFIG, BITS_DLPF_CFG_98HZ);
delay(1);
register_write(MPUREG_GYRO_CONFIG, BITS_GYRO_FS_2000DPS); // Gyro scale 2000º/s
delay(1);
_product_id = register_read(MPUREG_PRODUCT_ID); // read the product ID rev c has 1/2 the sensitivity of rev d
//Serial.printf("Product_ID= 0x%x\n", (unsigned) _product_id);
if ((_product_id == MPU6000ES_REV_C4) || (_product_id == MPU6000ES_REV_C5) ||
(_product_id == MPU6000_REV_C4) || (_product_id == MPU6000_REV_C5)){
// Accel scale 8g (4096 LSB/g)
// Rev C has different scaling than rev D
register_write(MPUREG_ACCEL_CONFIG,1<<3);
} else {
// Accel scale 8g (4096 LSB/g)
register_write(MPUREG_ACCEL_CONFIG,2<<3);
}
delay(1);
register_write(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // configure interrupt to fire when new data arrives
delay(1);
register_write(MPUREG_INT_PIN_CFG, BIT_INT_RD_CLEAR); // clear interrupt on any read
delay(1);
attachInterrupt(6,data_interrupt,RISING);
// initialise DMP. Should we only do this when we know we want to use the DMP for attitude sensing as well?
dmp_init();
}
static uint64_t rpu_read(void *opaque, hwaddr addr, unsigned size)
{
RPU *s = XILINX_RPU(opaque);
RegisterInfo *r = &s->regs_info[addr / 4];
if (!r->data) {
qemu_log("%s: Decode error: read from %" HWADDR_PRIx "\n",
object_get_canonical_path(OBJECT(s)),
addr);
return 0;
}
return register_read(r);
}
static gint
handle_cmd_write(bitstream_parsed_t *parsed,
bitstream_parser_t *parser) {
cmd_code_t cmd = register_read(parser, CMD);
switch(cmd) {
case MFW:
debit_log(L_BITSTREAM,"Executing multi-frame write");
record_frame(parsed, parser, register_read(parser, FAR));
break;
case RCRC:
debit_log(L_BITSTREAM,"Resetting CRC");
register_write(parser, CRC, 0);
break;
default:
debit_log(L_BITSTREAM,"execution of %i:%s is a noop",
cmd, cmd_names[cmd]);
break;
}
return 0;
}
static uint64_t gic_proxy_read(void *opaque, hwaddr addr, unsigned size)
{
GICProxy *s = XILINX_GIC_PROXY(opaque);
RegisterInfo *r = &s->regs_info[addr / 4];
if (!r->data) {
qemu_log_mask(LOG_GUEST_ERROR,
"%s: Decode error: read from %" HWADDR_PRIx "\n",
object_get_canonical_path(OBJECT(s)),
addr);
return 0;
}
return register_read(r);
}
int Simulator::simulate(){
load_i_cache();
load_d_cache();
print_d_cache();
print_i_cache();
while(1){
out.open("ins_pipeline.txt",ios::out);
fetch(ins_index[0]);
decode(ins_index[1]);
register_read(ins_index[2]);
execute(ins_index[3]);
mem_branch_cycle(ins_index[4]);
if (!write_back(ins_index[5])) {
cout << "Simulation Ends!" << endl;
break;
}
if(unset_raw_flag_cycle) {
unset_raw_flag_cycle = false;
if (!control_flag && !prev_raw_flag) {
ins_pipeline[ins_index[0]] = temp_fetch;
}
prev_raw_flag = false;
ins_pipeline[ins_index[1]] = temp_decode;
}
if(branch_pred_enabled) {
if (flush_pipeline) {
flush();
}
}
out << (m_clk + 1) << endl;
out.close();
system ("python gui.py");
next_clock_cycle();
//print_reg_file();
}
print_d_cache();
printf("clk cycles = %lld\n",m_clk + 1);
printf("CPI = %f\n",1.0*(m_clk + 1)/num_ins_executed);
printf("Stalls = %lld\n",num_stalls - 5);
if (!branch_pred_enabled){
printf("Control Stalls = %lld\n",num_control_stalls);
printf("RAW Stalls = %lld\n",num_stalls - 5 - num_control_stalls);
}
return 1;
}
static gint
idcode_write(bitstream_parsed_t *parsed,
bitstream_parser_t *parser) {
guint32 idcode = register_read(parser, IDCODE);
int i;
for (i = 0; i < XC_VLX__NUM; i++)
if (bitdescr[i].idcode == idcode) {
parser->type = i;
parsed->chip_struct = &bitdescr[i];
/* Allocate control structures */
alloc_indexer(parsed);
return 0;
}
g_warning("IDCODE %08x not recognized, aborting", idcode);
return -1;
}
uint8_t AP_HMC5843_SerialBus_I2C::read_raw(struct raw_value *rv)
{
return register_read(0x03, (uint8_t*)rv, sizeof(*rv));
}
