void rtm_set_param_reply(struct fins_module *module, struct finsFrame *ff) {
PRINT_DEBUG("Entered: module=%p, ff=%p, meta=%p", module, ff, ff->metaData);
struct rtm_data *md = (struct rtm_data *) module->data;
secure_sem_wait(&md->shared_sem);
struct rtm_command *cmd = (struct rtm_command *) list_find1(md->cmd_list, rtm_cmd_serial_test, &ff->ctrlFrame.serial_num);
if (cmd != NULL) {
list_remove(md->cmd_list, cmd);
struct rtm_console *console = (struct rtm_console *) list_find1(md->console_list, rtm_console_id_test, &cmd->console_id);
if (console != NULL) {
if (ff->ctrlFrame.ret_val == FCF_TRUE) {
rtm_send_text(console->fd, "successful");
} else {
//send error
uint32_t ret_msg;
secure_metadata_readFromElement(ff->metaData, "ret_msg", &ret_msg);
char temp[100];
sprintf(temp, "unsuccessful, returned error=%u", ret_msg);
rtm_send_text(console->fd, temp);
}
} else {
PRINT_WARN("todo error");
}
sem_post(&md->shared_sem);
free(cmd);
} else {
sem_post(&md->shared_sem);
PRINT_WARN("todo error");
//TODO error, drop
freeFinsFrame(ff);
}
}
int rtm_send_fd(int fd, uint32_t buf_len, uint8_t *buf) {
PRINT_DEBUG("Entered: fd=%d, buf_len=%u, buf='%s'", fd, buf_len, buf);
int numBytes = write(fd, &buf_len, sizeof(uint32_t));
if (numBytes <= 0) {
PRINT_DEBUG("Exited: fd=%d, buf_len=%u, buf='%s', %d", fd, buf_len, buf, numBytes);
return numBytes;
}
if (numBytes != sizeof(uint32_t)) {
PRINT_WARN("todo error");
PRINT_DEBUG("Exited: fd=%d, buf_len=%u, buf='%s', %d", fd, buf_len, buf, -1);
return -1;
}
numBytes = write(fd, buf, buf_len);
if (numBytes <= 0) {
PRINT_DEBUG("Exited: fd=%d, buf_len=%u, buf='%s', %d", fd, buf_len, buf, numBytes);
return numBytes;
}
if (buf_len != (uint32_t) numBytes) {
PRINT_WARN("todo error");
PRINT_DEBUG("Exited: fd=%d, buf_len=%u, buf='%s', %d", fd, buf_len, buf, -1);
return -1;
}
PRINT_DEBUG("Exited: fd=%d, buf_len=%u, buf='%s', %d", fd, buf_len, buf, numBytes);
return numBytes;
}
int memsector_load(memsector_handle_t *ms, const char *path) {
int status;
// load memory map
status = mmap_load(path, MAP_SHARED, &ms->mmap_handle);
if (status == -1) {
return -1;
}
// read and validate memsector data
memsector_header_t* memsector =
(memsector_header_t*) ms->mmap_handle.start_addr;
if (memsector->magic != MEMSECTOR_MAGIC) {
PRINT_WARN("File %s is not a memsector (magic mismatch)\n", path);
mmap_unload(&ms->mmap_handle);
return -1;
}
if (memsector->version != MEMSECTOR_VERSION) {
PRINT_WARN("Cannot process memsector %s v%d\n",
path, (int) memsector->version);
mmap_unload(&ms->mmap_handle);
return -1;
}
if (memsector->checksum != memsector_get_checksum(memsector)) {
PRINT_WARN("Memsector %s is corrupted (checksum mismatch)\n", path);
mmap_unload(&ms->mmap_handle);
return -1;
}
// initialize handle data
ms->ms = memsector;
return 0;
}
int memsector_create(memsector_writer_t *mswr,
const char *path) {
/* initialize memsector writer */
memset(mswr, 0, sizeof(*mswr));
/* create and resize file */
mswr->file = fopen(path, "w");
if (mswr->file == 0) {
PRINT_WARN("Error while opening %s: %s\n", path, strerror(errno));
return -1;
}
/* initialize header */
mswr->ms.magic = MEMSECTOR_MAGIC;
mswr->ms.version = MEMSECTOR_VERSION;
mswr->offset = sizeof(mswr->ms);
/* write header to memsector file */
size_t items_writen = fwrite(&mswr->ms, sizeof(mswr->ms), 1, mswr->file);
if (items_writen != 1) {
PRINT_WARN("Error while writing memsector header: %s\n",
strerror(errno));
fclose(mswr->file);
return -1;
}
return 0;
}
void switch_process_ff(struct fins_module *module, struct finsFrame *ff) {
PRINT_DEBUG("Entered: module=%p, ff=%p", module, ff);
if (ff->metaData == NULL) {
PRINT_ERROR("Error fcf.metadata==NULL");
exit(-1);
}
PRINT_WARN("TODO: switch process received frames: ff=%p, meta=%p", ff, ff->metaData);
print_finsFrame(ff);
if (ff->dataOrCtrl == FF_CONTROL) {
switch_fcf(module, ff);
PRINT_DEBUG("");
} else if (ff->dataOrCtrl == FF_DATA) {
if (ff->dataFrame.directionFlag == DIR_UP) {
//switch_in_fdf(module, ff);
PRINT_WARN("todo");
freeFinsFrame(ff);
} else if (ff->dataFrame.directionFlag == DIR_DOWN) {
//switch_out_fdf(ff);
PRINT_WARN("todo");
freeFinsFrame(ff);
} else {
PRINT_ERROR("todo error");
exit(-1);
}
} else {
PRINT_ERROR("todo error: dataOrCtrl=%u", ff->dataOrCtrl);
exit(-1);
}
}
int rtm_recv_fd(int fd, uint32_t buf_len, uint8_t *buf) {
PRINT_DEBUG("Entered: fd=%d, buf_len=%u, buf=%p", fd, buf_len, buf);
uint32_t msg_len;
int numBytes = read(fd, &msg_len, sizeof(uint32_t));
if (numBytes <= 0) {
PRINT_DEBUG("Exited: fd=%d, buf_len=%u, buf=%p, %d", fd, buf_len, buf, numBytes);
return numBytes;
}
if (numBytes != sizeof(uint32_t) || msg_len > buf_len) {
PRINT_WARN("todo error");
PRINT_DEBUG("Exited: fd=%d, buf_len=%u, buf=%p, %d", fd, buf_len, buf, -1);
return -1;
}
numBytes = read(fd, buf, msg_len);
if (numBytes <= 0) {
PRINT_DEBUG("Exited: fd=%d, buf_len=%u, buf=%p, %d", fd, buf_len, buf, numBytes);
return numBytes;
}
if (msg_len != (uint32_t) numBytes) {
PRINT_WARN("todo error");
PRINT_DEBUG("Exited: fd=%d, buf_len=%u, buf=%p, %d", fd, buf_len, buf, -1);
return -1;
}
PRINT_DEBUG("Exited: fd=%d, buf_len=%u, buf='%s', %d", fd, buf_len, buf, numBytes);
return numBytes;
}
void
EventReader::printData4Mathematica (unsigned int eventIdx, unsigned int trackIdx)
{
Track_t track;
scalar_t Vxx, Vyy, Vxy, X, Y, Phi, Theta, QoverP;
if (eventIdx >= mEvents.size ( )) {
PRINT_WARN ("eventIdx out of bounds!");
} else if (trackIdx >= mEvents.at (eventIdx).size ( )) {
PRINT_WARN ("trackIdx out of bounds!");
} else {
track = mEvents.at (eventIdx).at (trackIdx);
}
TrackData_t trackData;
for (size_t i = 0; i < trackData.size ( ); i++) {
printf ("F%lu =\n", i);
printf ("{");
for (int j = 0; j < ORDER; j++) {
printf ("{");
for (int k = 0; k < ORDER; k++) {
if (k != 4)
printf ("%f,", trackData.at (i).jacobi[j * ORDER + k]);
else
printf ("%f", trackData.at (i).jacobi[j * ORDER + k]);
}
if (j != 4)
printf ("},");
else
printf ("}");
}
printf ("};\n");
Vxx = trackData.at (i).err_locX * trackData.at (i).err_locX;
Vyy = trackData.at (i).err_locY * trackData.at (i).err_locY;
Vxy = trackData.at (i).cov_locXY;
printf ("V%lu =\n", i);
printf ("{{%f,%f,0,0,0},{%f,%f,0,0,0},{0,0,0,0,0},{0,0,0,0,0},{0,0,0,0,0}};\n",
Vxx, Vxy, Vxy, Vyy);
X = trackData.at (i).normal[0] - trackData.at (i).ref[0];
Y = trackData.at (i).normal[1] - trackData.at (i).ref[1];
Phi = trackData.at (i).normal[2] - trackData.at (i).ref[2];
Theta = trackData.at (i).normal[3] - trackData.at (i).ref[3];
QoverP = trackData.at (i).normal[4] - trackData.at (i).ref[4];
printf ("m%lu =\n", i);
printf ("{{%f}, {%f}, {%f}, {%f}, {%f}};\n", X, Y, Phi, Theta, QoverP);
printf ("\n");
}
}
void rtm_get_ff(struct fins_module *module) {
struct rtm_data *md = (struct rtm_data *) module->data;
struct finsFrame *ff;
do {
secure_sem_wait(module->event_sem);
secure_sem_wait(module->input_sem);
ff = read_queue(module->input_queue);
sem_post(module->input_sem);
} while (module->state == FMS_RUNNING && ff == NULL && !md->interrupt_flag); //TODO change logic here, combine with switch_to_rtm?
if (module->state != FMS_RUNNING) {
if (ff != NULL) {
freeFinsFrame(ff);
}
return;
}
if (ff != NULL) {
if (ff->metaData == NULL) {
PRINT_ERROR("Error fcf.metadata==NULL");
exit(-1);
}
if (ff->dataOrCtrl == FF_CONTROL) {
rtm_fcf(module, ff);
PRINT_DEBUG("");
} else if (ff->dataOrCtrl == FF_DATA) {
if (ff->dataFrame.directionFlag == DIR_UP) {
//rtm_in_fdf(module, ff);
PRINT_WARN("todo error");
freeFinsFrame(ff);
} else if (ff->dataFrame.directionFlag == DIR_DOWN) {
//rtm_out_fdf(module, ff);
PRINT_WARN("todo error");
freeFinsFrame(ff);
} else {
PRINT_ERROR("todo error");
exit(-1);
}
} else {
PRINT_ERROR("todo error");
exit(-1);
}
} else if (md->interrupt_flag) {
md->interrupt_flag = 0;
rtm_interrupt(module); //TODO unused, implement or remove
} else {
PRINT_ERROR("todo error: dataOrCtrl=%u", ff->dataOrCtrl);
exit(-1);
}
}
void ipv4_fcf(struct fins_module *module, struct finsFrame *ff) {
PRINT_DEBUG("Entered: module=%p, ff=%p, meta=%p", module, ff, ff->metaData);
//TODO make sure is meant for IP, filter out FCF sent through flows w/multi dest
//TODO fill out
switch (ff->ctrlFrame.opcode) {
case CTRL_ALERT:
PRINT_DEBUG("opcode=CTRL_ALERT (%d)", CTRL_ALERT);
PRINT_WARN("todo");
module_reply_fcf(module, ff, FCF_FALSE, 0);
break;
case CTRL_ALERT_REPLY:
PRINT_DEBUG("opcode=CTRL_ALERT_REPLY (%d)", CTRL_ALERT_REPLY);
PRINT_WARN("todo");
freeFinsFrame(ff);
break;
case CTRL_READ_PARAM:
PRINT_DEBUG("opcode=CTRL_READ_PARAM (%d)", CTRL_READ_PARAM);
ipv4_read_param(module, ff);
break;
case CTRL_READ_PARAM_REPLY:
PRINT_DEBUG("opcode=CTRL_READ_PARAM_REPLY (%d)", CTRL_READ_PARAM_REPLY);
PRINT_WARN("todo");
freeFinsFrame(ff);
break;
case CTRL_SET_PARAM:
PRINT_DEBUG("opcode=CTRL_SET_PARAM (%d)", CTRL_SET_PARAM);
ipv4_set_param(module, ff);
break;
case CTRL_SET_PARAM_REPLY:
PRINT_DEBUG("opcode=CTRL_SET_PARAM_REPLY (%d)", CTRL_SET_PARAM_REPLY);
PRINT_WARN("todo");
freeFinsFrame(ff);
break;
case CTRL_EXEC:
PRINT_DEBUG("opcode=CTRL_EXEC (%d)", CTRL_EXEC);
ipv4_exec(module, ff);
break;
case CTRL_EXEC_REPLY:
PRINT_DEBUG("opcode=CTRL_EXEC_REPLY (%d)", CTRL_EXEC_REPLY);
ipv4_exec_reply(module, ff);
break;
case CTRL_ERROR:
PRINT_DEBUG("opcode=CTRL_ERROR (%d)", CTRL_ERROR);
ipv4_error(module, ff);
break;
default:
PRINT_ERROR("opcode=default (%d)", ff->ctrlFrame.opcode);
exit(-1);
break;
}
}
void SchemaQueryHandler::getSubstitutions(CmmlToken* exprRoot,
CmmlToken* schemaRoot,
vector<string>* subst) {
if (exprRoot == nullptr || schemaRoot == nullptr || subst == nullptr) {
PRINT_WARN("nullptr pased. Skipping...");
return;
}
auto exprChildren = exprRoot->getChildNodes();
auto schemaChildren = schemaRoot->getChildNodes();
const string& currSchemaTag = schemaRoot->getTag();
if (currSchemaTag == types::QVAR_TAG) {
const string& xref = exprRoot->getAttribute("xref");
if (xref == "") {
PRINT_LOG("Missing href attribute. Skipping...");
return;
}
subst->push_back(xref);
return;
}
if (exprChildren.size() != schemaChildren.size()) {
PRINT_WARN("Expression and schema are incompatible. Skipping...");
return;
}
auto expr_it = exprChildren.begin();
auto schema_it = schemaChildren.begin();
const string& currExprTag = exprRoot->getTag();
if (currExprTag != currSchemaTag) {
PRINT_WARN("Expression and schema are incompatible. Skipping...");
return;
}
// Done with this token?
if (exprChildren.size() == 0) {
return;
}
// disregard the first child of apply
if (currExprTag == "apply") {
expr_it++;
schema_it++;
}
/* we know that both exprChildren and schemaChildren have the same length,
* so it is enough to check for one */
while (expr_it != exprChildren.end()) {
getSubstitutions(*expr_it, *schema_it, subst);
++expr_it;
++schema_it;
}
}
void rtm_fcf(struct fins_module *module, struct finsFrame *ff) {
PRINT_DEBUG("Entered: module=%p, ff=%p, meta=%p", module, ff, ff->metaData);
//TODO when recv FCF, pull params from meta to figure out connection, send through socket
switch (ff->ctrlFrame.opcode) {
case CTRL_ALERT:
PRINT_DEBUG("opcode=CTRL_ALERT (%d)", CTRL_ALERT);
rtm_alert(module, ff);
break;
case CTRL_ALERT_REPLY:
PRINT_DEBUG("opcode=CTRL_ALERT_REPLY (%d)", CTRL_ALERT_REPLY);
PRINT_WARN("todo");
freeFinsFrame(ff);
break;
case CTRL_READ_PARAM:
PRINT_DEBUG("opcode=CTRL_READ_PARAM (%d)", CTRL_READ_PARAM);
PRINT_WARN("todo");
module_reply_fcf(module, ff, FCF_FALSE, 0);
break;
case CTRL_READ_PARAM_REPLY:
PRINT_DEBUG("opcode=CTRL_READ_PARAM_REPLY (%d)", CTRL_READ_PARAM_REPLY);
rtm_read_param_reply(module, ff);
break;
case CTRL_SET_PARAM:
PRINT_DEBUG("opcode=CTRL_SET_PARAM (%d)", CTRL_SET_PARAM);
rtm_set_param(module, ff);
break;
case CTRL_SET_PARAM_REPLY:
PRINT_DEBUG("opcode=CTRL_SET_PARAM_REPLY (%d)", CTRL_SET_PARAM_REPLY);
rtm_set_param_reply(module, ff);
break;
case CTRL_EXEC:
PRINT_DEBUG("opcode=CTRL_EXEC (%d)", CTRL_EXEC);
PRINT_WARN("todo");
module_reply_fcf(module, ff, FCF_FALSE, 0);
break;
case CTRL_EXEC_REPLY:
PRINT_DEBUG("opcode=CTRL_EXEC_REPLY (%d)", CTRL_EXEC_REPLY);
rtm_exec_reply(module, ff);
break;
case CTRL_ERROR:
PRINT_DEBUG("opcode=CTRL_ERROR (%d)", CTRL_ERROR);
PRINT_WARN("todo");
freeFinsFrame(ff);
break;
default:
PRINT_ERROR("opcode=default (%d)", ff->ctrlFrame.opcode);
exit(-1);
break;
}
}
static int
tape_34xx_erp_bug(struct tape_device *device, struct tape_request *request,
struct irb *irb, int no)
{
if (request->op != TO_ASSIGN) {
PRINT_WARN("An unexpected condition #%d was caught in "
"tape error recovery.\n", no);
PRINT_WARN("Please report this incident.\n");
if (request)
PRINT_WARN("Operation of tape:%s\n",
tape_op_verbose[request->op]);
tape_dump_sense(device, request, irb);
}
return tape_34xx_erp_failed(request, -EIO);
}
static inline int setupCopyToStringWriter(MwsHarvest_SaxUserData* data) {
xmlOutputBuffer* outPtr;
data->buffer.clear();
if ((outPtr = xmlOutputBufferCreateIO(copyToCharBufCallback, nullptr,
&data->buffer, nullptr)) == nullptr) {
PRINT_WARN("Error while creating the OutputBuffer\n");
return -1;
} else if ((data->stringWriter = xmlNewTextWriter(outPtr)) == nullptr) {
PRINT_WARN("Error while creating the XML Writer\n");
return -1;
}
return 0;
}
// Predict the state ahead using the formula:
// P_(k|k-1) = F_(k) P_(k|k-1)
int
KalmanFilterSerial::predictState (GSL_VECTOR *dest, GSL_MATRIX *F_k, GSL_VECTOR *P_k1) const
{
if (!dest || !F_k || !P_k1) {
PRINT_WARN ("Parameter is NULL!");
return -1;
}
#if DBG_LVL > 2
std::cout << "predictState:" << std::endl;
std::cout << "F_k: ";
printMatrix (F_k);
std::cout << "P_k: ";
printVector (P_k1);
#endif
GSL_BLAS_XGEMV (CblasNoTrans, 1.0f, F_k, P_k1, 0.0f, dest);
#if DBG_LVL > 2
std::cout << "maxima predict State:" << std::endl;
std::cout << "F_k . P_k;" << std::endl;
std::cout << "res: ";
printVector (dest);
#endif
return 0;
}
bool Ingridients::saveData(QString& fileName) {
QJsonObject saveObj;
QFile saveFile(fileName);
PRINT_DEBUG("Saving all ingridients");
if ( !saveFile.open(QIODevice::WriteOnly) ) {
PRINT_WARN("Couldn't open save file.");
return false;
}
saveItems(saveObj);
saveFood(saveObj);
saveDish(saveObj);
QJsonDocument saveDoc(saveObj);
if ( !fileName.right(5).compare(".json") ) {
saveFile.write(saveDoc.toJson());
} else {
saveFile.write(saveDoc.toBinaryData());
}
return true;
}
void udp_exec(struct fins_module *module, struct finsFrame *ff) {
PRINT_DEBUG("Entered: module=%p, ff=%p, meta=%p", module, ff, ff->metaData);
uint32_t host_ip = 0;
uint32_t host_port = 0;
uint32_t rem_ip = 0;
uint32_t rem_port = 0;
switch (ff->ctrlFrame.param_id) {
case UDP_EXEC_CLEAR_SENT:
PRINT_DEBUG("param_id=UDP_EXEC_CLEAR_SENT (%d)", ff->ctrlFrame.param_id);
secure_metadata_readFromElement(ff->metaData, "host_ip", &host_ip);
secure_metadata_readFromElement(ff->metaData, "host_port", &host_port);
secure_metadata_readFromElement(ff->metaData, "rem_ip", &rem_ip);
secure_metadata_readFromElement(ff->metaData, "rem_port", &rem_port);
udp_exec_clear_sent(module, ff, host_ip, (uint16_t) host_port, rem_ip, (uint16_t) rem_port);
break;
default:
PRINT_ERROR("Error unknown param_id=%d", ff->ctrlFrame.param_id);
PRINT_WARN("todo");
module_reply_fcf(module, ff, FCF_FALSE, 0);
break;
}
}
void module_set_param_links(struct fins_module *module, struct finsFrame *ff) {
PRINT_DEBUG("Entered: module=%p, ff=%p, meta=%p", module, ff, ff->metaData);
struct fins_module_table *mt = (struct fins_module_table *) module->data;
if (ff->ctrlFrame.data_len != sizeof(struct linked_list)) {
PRINT_WARN("todo error");
freeFinsFrame(ff);
return;
}
if (mt->link_list != NULL) {
list_free(mt->link_list, free);
}
mt->link_list = (struct linked_list *) ff->ctrlFrame.data;
int i;
for (i = 0; i < mt->flows_num; i++) {
mt->flows[i].link = (struct link_record *) list_find1(mt->link_list, link_id_test, &mt->flows[i].link_id);
}
#ifdef DEBUG
list_for_each(mt->link_list, link_print);
#endif
ff->ctrlFrame.data = NULL;
freeFinsFrame(ff);
}
void ipv4_error(struct fins_module *module, struct finsFrame *ff) {
PRINT_DEBUG("Entered: module=%p, ff=%p, meta=%p", module, ff, ff->metaData);
switch (ff->ctrlFrame.param_id) {
case IPV4_ERROR_GET_ADDR:
PRINT_DEBUG("param_id=IPV4_ERROR_GET_ADDR (%d)", ff->ctrlFrame.param_id);
//should we separate icmp & error messages? what about disabling ICMP, what errors should it stop?
//if yes, eth->ip->icmp or ip->proto
//if no, eth->icmp->proto
//if partial, eth->ip->icmp->proto (allows for similar to iptables)
//Sending to ICMP mimic kernel func, if remove icmp stops error
//if doing routing tables, factor in and process
ff->ctrlFrame.sender_id = module->index;
ff->ctrlFrame.param_id = IPV4_ERROR_GET_ADDR;
//reroute to icmp, though could go directly
int sent = module_send_flow(module, ff, IPV4_FLOW_ICMP);
if (sent == 0) {
freeFinsFrame(ff);
}
break;
default:
PRINT_DEBUG("param_id=default (%d)", ff->ctrlFrame.param_id);
PRINT_WARN("todo");
freeFinsFrame(ff);
break;
}
}
void module_set_param_flows(struct fins_module *module, struct finsFrame *ff) {
PRINT_DEBUG("Entered: module=%p, ff=%p, meta=%p", module, ff, ff->metaData);
struct fins_module_table *mt = (struct fins_module_table *) module->data;
if (module->flows_max < ff->ctrlFrame.data_len) {
PRINT_WARN("todo error");
freeFinsFrame(ff);
return;
}
mt->flows_num = ff->ctrlFrame.data_len;
struct fins_module_flow *flows = (struct fins_module_flow *) ff->ctrlFrame.data;
int i;
for (i = 0; i < mt->flows_num; i++) {
mt->flows[i].link_id = flows[i].link_id;
mt->flows[i].link = (struct link_record *) list_find1(mt->link_list, link_id_test, &mt->flows[i].link_id);
}
#ifdef DEBUG
uint8_t buf[1000];
uint8_t *pt = buf;
int ret;
for (i = 0; i < mt->flows_num; i++) {
//ret = sprintf((char *) pt, "%u, ", mt->flows[i].link_id);
ret = sprintf((char *) pt, "%u (%p), ", mt->flows[i].link_id, mt->flows[i].link);
pt += ret;
}
*pt = '\0';
PRINT_DEBUG("flows: max=%u, num=%u, ['%s']", module->flows_max, mt->flows_num, buf);
#endif
//freeFF frees flows
freeFinsFrame(ff);
}
/*
* Get dump parameters from /proc/cmdline
* Return: 0 - ok
* (!= 0) - error
*/
static int parse_parmline(void)
{
int fh, i, count, token_cnt;
char *token;
char *parms[KERN_PARM_MAX];
/* setting defaults */
g.parm_compress = PARM_COMP_DFLT;
g.parm_dir = PARM_DIR_DFLT;
g.parm_part = PARM_PART_DFLT;
g.parm_debug = PARM_DEBUG_DFLT;
g.parm_mode = PARM_MODE_NUM_DFLT;
g.parm_mem = PARM_MEM_DFLT;
fh = open(PROC_CMDLINE, O_RDONLY);
if (fh == -1) {
PRINT_PERR("open %s failed\n", PROC_CMDLINE);
return -1;
}
count = read(fh, g.parmline, CMDLINE_MAX_LEN);
if (count == -1) {
PRINT_PERR("read %s failed\n", PROC_CMDLINE);
close(fh);
return -1;
}
g.parmline[count-1] = '\0'; /* remove \n */
token_cnt = 0;
token = strtok(g.parmline, " \t\n");
while (token != NULL) {
parms[token_cnt] = token;
token = strtok(NULL, " \t\n");
token_cnt++;
if (token_cnt >= KERN_PARM_MAX) {
PRINT_WARN("More than %i kernel parmameters "
"specified\n", KERN_PARM_MAX);
break;
}
}
for (i = 0; i < token_cnt; i++) {
if (parse_parameter(parms[i])) {
close(fh);
return -1;
}
}
PRINT_TRACE("dump dir : %s\n", g.parm_dir);
PRINT_TRACE("dump part : %s\n", g.parm_part);
PRINT_TRACE("dump comp : %s\n", g.parm_compress);
PRINT_TRACE("dump debug: %d\n", g.parm_debug);
PRINT_TRACE("dump mem: %llx\n", (unsigned long long) g.parm_mem);
if (g.parm_mode == PARM_MODE_AUTO_NUM)
PRINT_TRACE("dump mode : %s\n", PARM_MODE_AUTO);
if (g.parm_mode == PARM_MODE_INTERACT_NUM)
PRINT_TRACE("dump mode : %s\n", PARM_MODE_INTERACT);
sprintf(g.dump_dir, "%s/%s", DUMP_DIR, g.parm_dir);
close(fh);
return 0;
}
static fpos_t
memstream_seek(void *cookie, fpos_t pos, int whence)
{
struct memstream *ms;
#ifdef DEBUG
size_t old;
#endif
ms = cookie;
#ifdef DEBUG
old = ms->offset;
#endif
switch (whence) {
case SEEK_SET:
ms->offset = pos;
break;
case SEEK_CUR:
ms->offset += pos;
break;
case SEEK_END:
ms->offset = *ms->lenp + pos;
break;
}
#ifdef DEBUG
PRINT_WARN("MS: seek(%p, %zd, %d) %zd -> %zd\n", ms, pos, whence,
old, ms->offset);
#endif
return (ms->offset);
}
static int __init vmcp_init(void)
{
int ret;
if (!MACHINE_IS_VM) {
PRINT_WARN("z/VM CP interface is only available under z/VM\n");
return -ENODEV;
}
vmcp_debug = debug_register("vmcp", 1, 1, 240);
if (!vmcp_debug)
return -ENOMEM;
ret = debug_register_view(vmcp_debug, &debug_hex_ascii_view);
if (ret) {
debug_unregister(vmcp_debug);
return ret;
}
ret = misc_register(&vmcp_dev);
if (ret) {
debug_unregister(vmcp_debug);
return ret;
}
return 0;
}
HarvestResult processHarvestFromFd(int fd, HarvestProcessor* harvestProcessor,
bool shouldProcessData) {
MwsHarvest_SaxUserData user_data;
xmlSAXHandler saxHandler;
xmlParserCtxtPtr ctxtPtr;
HarvestResult result;
result.status = -1;
user_data.harvestProcessor = harvestProcessor;
user_data.shouldProcessData = shouldProcessData;
memset(&saxHandler, 0, sizeof(xmlSAXHandler));
// Registering Sax callbacks
saxHandler.endDocument = my_endDocument;
saxHandler.startElement = my_startElement;
saxHandler.endElement = my_endElement;
saxHandler.characters = my_characters;
saxHandler.warning = my_warning;
saxHandler.error = my_error;
saxHandler.fatalError = my_fatalError;
// Locking libXML -- to allow multi-threaded use
xmlLockLibrary();
// Creating the IOParser context
if ((ctxtPtr = xmlCreateIOParserCtxt(&saxHandler, &user_data,
fdXmlInputReadCallback, nullptr, &fd,
XML_CHAR_ENCODING_UTF8)) ==
nullptr) {
PRINT_WARN("Error while creating the ParserContext\n");
}
// Parsing the document
else if ((result.status = xmlParseDocument(ctxtPtr)) == -1) {
PRINT_WARN("Parsing XML document failed\n");
}
// Freeing the parser context
if (ctxtPtr) {
xmlFreeParserCtxt(ctxtPtr);
}
// Unlocking libXML -- to allow multi-threaded use
xmlUnlockLibrary();
result.numExpressions = user_data.parsedExpr;
return result;
}
// Return the data for the track trackIdx associated with the event eventIdx
Track_t
EventReader::getTrack (unsigned int eventIdx, unsigned int trackIdx)
{
Track_t track;
if (eventIdx >= mEvents.size ( )) {
PRINT_WARN ("eventIdx out of bounds!");
return track;
}
if (trackIdx >= mEvents.at (eventIdx).size ( )) {
PRINT_WARN ("trackIdx out of bounds!");
return track;
}
return mEvents.at (eventIdx).at (trackIdx);
}
void
EventReader::printC (unsigned int eventIdx,
unsigned int trackIdx, scalar_t *C)
{
Track_t track;
int i, j, k;
int hitCount;
if (eventIdx >= mEvents.size ( )) {
PRINT_WARN ("eventIdx out of bounds!");
} else if (trackIdx >= mEvents.at (eventIdx).size ( )) {
PRINT_WARN ("trackIdx out of bounds!");
} else {
track = mEvents.at (eventIdx).at (trackIdx);
}
TrackData_t trackData;
trackData = track.track;
hitCount = trackData.size();
for (i = 0; i< hitCount; i++) {
printf("%d\n",i);
for(j=0; j<ORDER; j++){
for(k=0; k<ORDER; k++){
printf("%.10f\t",C[(j*ORDER+k)+(i*ORDER*ORDER)] );
}
printf("\n");
}
printf("\n");
}
printf("\n\n");
for (i = 0; i< hitCount; i++) {
printf("%d\n",i);
for(j=0; j<ORDER; j++){
for(k=0; k<ORDER; k++){
printf("%.10f\t",C[(j*ORDER+k)+(i*ORDER*ORDER)+(ORDER*ORDER*hitCount)] );
}
printf("\n");
}
printf("\n");
}
printf("\n\n");
}
void switch_fcf(struct fins_module *module, struct finsFrame *ff) {
PRINT_DEBUG("Entered: module=%p, ff=%p, meta=%p", module, ff, ff->metaData);
//TODO fill out
switch (ff->ctrlFrame.opcode) {
case CTRL_ALERT:
PRINT_DEBUG("opcode=CTRL_ALERT (%d)", CTRL_ALERT);
PRINT_WARN("todo");
module_reply_fcf(module, ff, FCF_FALSE, 0);
break;
case CTRL_ALERT_REPLY:
PRINT_DEBUG("opcode=CTRL_ALERT_REPLY (%d)", CTRL_ALERT_REPLY);
PRINT_WARN("todo");
freeFinsFrame(ff);
break;
case CTRL_READ_PARAM:
PRINT_DEBUG("opcode=CTRL_READ_PARAM (%d)", CTRL_READ_PARAM);
PRINT_WARN("todo");
module_reply_fcf(module, ff, FCF_FALSE, 0);
break;
case CTRL_READ_PARAM_REPLY:
PRINT_DEBUG("opcode=CTRL_READ_PARAM_REPLY (%d)", CTRL_READ_PARAM_REPLY);
PRINT_WARN("todo");
freeFinsFrame(ff);
break;
case CTRL_SET_PARAM:
PRINT_DEBUG("opcode=CTRL_SET_PARAM (%d)", CTRL_SET_PARAM);
switch_set_param(module, ff);
break;
case CTRL_SET_PARAM_REPLY:
PRINT_DEBUG("opcode=CTRL_SET_PARAM_REPLY (%d)", CTRL_SET_PARAM_REPLY);
PRINT_WARN("todo");
freeFinsFrame(ff);
break;
case CTRL_EXEC:
PRINT_DEBUG("opcode=CTRL_EXEC (%d)", CTRL_EXEC);
PRINT_WARN("todo");
module_reply_fcf(module, ff, FCF_FALSE, 0);
break;
case CTRL_EXEC_REPLY:
PRINT_DEBUG("opcode=CTRL_EXEC_REPLY (%d)", CTRL_EXEC_REPLY);
PRINT_WARN("todo");
freeFinsFrame(ff);
break;
case CTRL_ERROR:
PRINT_DEBUG("opcode=CTRL_ERROR (%d)", CTRL_ERROR);
PRINT_WARN("todo");
freeFinsFrame(ff);
break;
default:
PRINT_ERROR("opcode=default (%d)", ff->ctrlFrame.opcode);
exit(-1);
break;
}
}
// Return the number of tracks associated with the event eventIdx
int
EventReader::getTrackCount (unsigned int eventIdx)
{
if (eventIdx >= mEvents.size ( )) {
PRINT_WARN ("eventIdx out of bounds!");
return 0;
}
return mEvents.at (eventIdx).size ( );
}
// Update the error covariance using the formula:
// C_(k|k) = (I - K_(k) H_(k)) C_(k|k-1)
int
KalmanFilterSerial::correctCovariance (GSL_MATRIX *dest, GSL_MATRIX *K_k,
GSL_MATRIX *H_k, GSL_MATRIX *C_k1) const
{
if (!dest || !K_k || !H_k || !C_k1) {
PRINT_WARN ("Parameter is NULL!");
return -1;
}
#if DBG_LVL > 2
std::cout << "correctCovariance:" << std::endl;
std::cout << "H_k: ";
printMatrix (H_k);
std::cout << "K_k: ";
printMatrix (K_k);
std::cout << "C_k: ";
printMatrix (C_k1);
#endif
GSL_MATRIX *tmp, *identity;
tmp = GSL_MATRIX_CALLOC (K_k->size1, K_k->size1);
identity = GSL_MATRIX_CALLOC (K_k->size1, K_k->size1);
GSL_MATRIX_SET_IDENTITY (identity);
// tmp = K_(k) * H_(k)
GSL_BLAS_XGEMM (CblasNoTrans, CblasNoTrans, 1.0, K_k, H_k, 0.0, tmp);
#if DBG_LVL > 2
std::cout << "tmp: ";
printMatrix (tmp);
std::cout << "identity: ";
printMatrix (identity);
#endif
// identity = identity - tmp
GSL_MATRIX_SUB (identity, tmp);
// dest = identity * C_(k|k-1)
GSL_BLAS_XGEMM (CblasNoTrans, CblasNoTrans, 1.0, identity, C_k1, 0.0, dest);
GSL_MATRIX_FREE (identity);
GSL_MATRIX_FREE (tmp);
#if DBG_LVL > 2
std::cout << "maxima correct Covariance:" << std::endl;
std::cout << "(ident (5) - K_k . H_k) . C_k;" << std::endl;
std::cout << "res: ";
printMatrix (dest);
#endif
return 0;
}
void EventReader::print(unsigned int eventIdx, unsigned int trackIdx)
{
size_t i, j;
if (eventIdx >= mEvents.size()) {
PRINT_WARN("eventIdx out of bounds!");
return;
} else if (trackIdx >= mEvents.at(eventIdx).size()) {
PRINT_WARN("trackIdx out of bounds!");
return;
}
Track_t& track = mEvents.at(eventIdx).at(trackIdx);
std::cout << "event " << eventIdx << " track " << trackIdx<< std::endl;
std::cout << "\tNormalLocX\tRefLocX\t\tNormalLocY\t\tRefLocY\t\tRefQoverP\t\tdetType\t\tmeasType\t\tsigmaDeltaP\t\tsigmaDeltaTheta"
<< std::endl;
TrackData_t& trackData = track.track;
for (i = 0; i < trackData.size(); i++) {
/*myfile << trackData.at (i).measurement[0] << ";"
<< trackData.at (i).ref[0] << ";" << XFit[i] << ";"
<< trackData.at (i).measurement[1] << ";"
<< trackData.at (i).ref[1] << ";" << YFit[i] << ";"
<< trackData.at (i).detType << std::endl;
*/
printf("%lu.\t%8.4f\t%8.4f\t%8.4f\t%8.4f\t%4.3e\t%d\t%d\t%4.3e\t%4.3e\n", i,
trackData.at(i).normal[0], trackData.at(i).ref[0],
trackData.at(i).normal[1], trackData.at(i).ref[1],
trackData.at(i).ref[4],
trackData.at(i).detType, trackData.at(i).is2Dim,
#if HAS_MATERIAL_EFFECTS
trackData.at(i).sigmaDeltaQoverPSquared, trackData.at(i).sigmaDeltaThetaSquared);
#else
0., 0.);
#endif
}
printf("\n\n");
}
void module_set_param_dual(struct fins_module *module, struct finsFrame *ff) {
PRINT_DEBUG("Entered: module=%p, ff=%p, meta=%p", module, ff, ff->metaData);
struct fins_module_table *mt = (struct fins_module_table *) module->data;
if (ff->ctrlFrame.data_len != sizeof(struct fins_module_table)) {
PRINT_WARN("todo error");
freeFinsFrame(ff);
return;
}
struct fins_module_table *table = (struct fins_module_table *) ff->ctrlFrame.data;
if (module->flows_max < table->flows_num || table->link_list == NULL) {
PRINT_WARN("todo error");
freeFinsFrame(ff);
return;
}
mt->link_list = table->link_list;
mt->flows_num = table->flows_num;
if (mt->flows_num != 0) {
memcpy(mt->flows, table->flows, mt->flows_num * sizeof(struct fins_module_flow));
}
#ifdef DEBUG
uint8_t buf[1000];
uint8_t *pt = buf;
int ret;
int i;
for (i = 0; i < mt->flows_num; i++) {
//ret = sprintf((char *) pt, "%u, ", mt->flows[i].link_id);
ret = sprintf((char *) pt, "%u (%p), ", mt->flows[i].link_id, mt->flows[i].link);
pt += ret;
}
*pt = '\0';
PRINT_DEBUG("flows: max=%u, num=%u, ['%s']", module->flows_max, mt->flows_num, buf);
list_for_each(mt->link_list, link_print);
#endif
//freeFF frees table
freeFinsFrame(ff);
}
