// v11 -> v12 is more important
inline static float interpolate_square(MID_POINT_CONFIG *p_config,
float v11, float v12,
float v21, float v22,
SQUARE_INTERPOLATION_TYPE interpolation)
{
float result = 0;
switch(interpolation) {
case INTERPOLATION_MID_POINT:
result = interpolate_float(v11, v12, v21, v22);
break;
case INTERPOLATION_LINE_MIN:
case INTERPOLATION_LINE_MAX:
case INTERPOLATION_LINE_CENTER:
case INTERPOLATION_LINE_RANGE_HIGH:
case INTERPOLATION_LINE_RANGE_LOW:
case INTERPOLATION_LINE_PRIORITY_HIGH:
case INTERPOLATION_LINE_PRIORITY_LOW:
case INTERPOLATION_LINE_RANDOM:
{
float min1 = FLT_MAX;
float max1 = -FLT_MAX;
float min2 = FLT_MAX;
float max2 = -FLT_MAX;
float a1_height;
bool a1 = interpolate_axe(v11, v12, min1, max1, a1_height);
float a2_height;
bool a2 = interpolate_axe(v21, v22, min2, max2, a2_height);
if(!a1 && !a2) {
bool a = choose_one(v11, v12, v21, v22, result);
assert(a);
}
else if(a1 && a2) {
switch(interpolation) {
case INTERPOLATION_LINE_MIN:
result = MIN(a1_height, a2_height);
break;
case INTERPOLATION_LINE_MAX:
result = MAX(a1_height, a2_height);
break;
case INTERPOLATION_LINE_CENTER:
{
float center = interpolate_float(v11, v12, v21, v22);
if(abs(center-a1_height) <= abs(center-a2_height)) {
result = a1_height;
}
else {
result = a2_height;
}
break;
}
case INTERPOLATION_LINE_PRIORITY_HIGH:
result = a1_height;
break;
case INTERPOLATION_LINE_PRIORITY_LOW:
result = a2_height;
break;
case INTERPOLATION_LINE_RANGE_HIGH:
result = (abs(max1-min1) > abs(max2-min2)) ? a1_height : a2_height;
break;
case INTERPOLATION_LINE_RANGE_LOW:
result = (abs(max1-min1) < abs(max2-min2)) ? a1_height : a2_height;
break;
case INTERPOLATION_LINE_RANDOM:
result = (random_generator::generator_rand_0() > 0) ? a1_height : a2_height;
break;
default:
break;
}
pdebug(DEBUG_HEIGHT,"interpolation = %d, a1_height = %f, a2_height = %f, result = %f",
interpolation,a1_height,a2_height,result);
}
else if(a1) {
result = a1_height;
}
else if(a2) {
result = a2_height;
}
break;
}
}
if(p_config->perturbation != FLOAT_UNDEFINED) {
float min;
float max;
range_float(v11, v12, v21, v22, min, max);
float range = (max - min);
result += random_generator::generator_rand_0()*range*p_config->perturbation;
}
pdebug(DEBUG_HEIGHT,"[%f, %f, %f, %f] -> %f", v11, v12, v21, v22, result);
return(result);
}
static int check_read_status(ab_tag_p tag)
{
pccc_resp *pccc;
uint8_t *data;
uint8_t *data_end;
int pccc_res_type;
int pccc_res_length;
int rc = PLCTAG_STATUS_OK;
ab_request_p req;
int debug = tag->debug;
pdebug(debug,"Starting");
/* PCCC only can have one request outstanding */
/* is there an outstanding request? */
if(!tag->reqs || !(tag->reqs[0])) {
tag->read_in_progress = 0;
tag->status = PLCTAG_ERR_NULL_PTR;
return PLCTAG_ERR_NULL_PTR;
}
req = tag->reqs[0];
if(!req->resp_received) {
tag->status = PLCTAG_STATUS_PENDING;
return PLCTAG_STATUS_PENDING;
}
/* fake exceptions */
do {
pccc = (pccc_resp*)(req->data);
data_end = (req->data + pccc->encap_length + sizeof(eip_encap_t));
if(le2h16(pccc->encap_command) != AB_EIP_READ_RR_DATA) {
pdebug(debug,"Unexpected EIP packet type received: %d!",pccc->encap_command);
rc = PLCTAG_ERR_BAD_DATA;
break;
}
if(le2h16(pccc->encap_status) != AB_EIP_OK) {
pdebug(debug,"EIP command failed, response code: %d",pccc->encap_status);
rc = PLCTAG_ERR_REMOTE_ERR;
break;
}
if(pccc->general_status != AB_EIP_OK) {
pdebug(debug,"PCCC command failed, response code: %d",pccc->general_status);
rc = PLCTAG_ERR_REMOTE_ERR;
break;
}
if(pccc->pccc_status != AB_EIP_OK) {
/*pdebug(PLC_LOG_ERR,PLC_ERR_READ, "PCCC command failed, response code: %d",pccc_resp->pccc_status);*/
pdebug(debug,pccc_decode_error(pccc->pccc_data[0]));
rc = PLCTAG_ERR_REMOTE_ERR;
break;
}
/* point to the start of the data */
data = pccc->pccc_data;
if(!(data = pccc_decode_dt_byte(data,data_end - data, &pccc_res_type,&pccc_res_length))) {
pdebug(debug,"Unable to decode PCCC response data type and data size!");
rc = PLCTAG_ERR_BAD_DATA;
break;
}
/* this gives us the overall type of the response and the number of bytes remaining in it.
* If the type is an array, then we need to decode another one of these words
* to get the type of each element and the size of each element. We will
* need to adjust the size if we care.
*/
if(pccc_res_type == AB_PCCC_DATA_ARRAY) {
if(!(data = pccc_decode_dt_byte(data,data_end - data, &pccc_res_type,&pccc_res_length))) {
pdebug(debug,"Unable to decode PCCC response array element data type and data size!");
rc = PLCTAG_ERR_BAD_DATA;
break;
}
}
/* copy data into the tag. */
if((data_end - data) > tag->size) {
rc = PLCTAG_ERR_TOO_LONG;
break;
}
mem_copy(tag->data, data, data_end - data);
rc = PLCTAG_STATUS_OK;
} while(0);
/* get rid of the request now */
ab_tag_abort(tag);
tag->status = rc;
pdebug(debug,"Done.");
return rc;
}
/*
* check_write_status
*
* Fragments are not supported.
*/
static int check_write_status(ab_tag_p tag)
{
pccc_resp *pccc;
int rc = PLCTAG_STATUS_OK;
ab_request_p req;
int debug = tag->debug;
pdebug(debug,"Starting.");
/* is there an outstanding request? */
if(!tag->reqs || !(tag->reqs[0]) ) {
tag->write_in_progress = 0;
tag->status = PLCTAG_ERR_NULL_PTR;
return PLCTAG_ERR_NULL_PTR;
}
req = tag->reqs[0];
if(!req->resp_received) {
tag->status = PLCTAG_STATUS_PENDING;
return PLCTAG_STATUS_PENDING;
}
/* fake exception */
do {
pccc = (pccc_resp*)(req->data);
/* check the response status */
if( le2h16(pccc->encap_command) != AB_EIP_READ_RR_DATA) {
pdebug(debug,"EIP unexpected response packet type: %d!",pccc->encap_command);
rc = PLCTAG_ERR_BAD_DATA;
break;
}
if(le2h16(pccc->encap_status) != AB_EIP_OK) {
pdebug(debug,"EIP command failed, response code: %d",pccc->encap_status);
rc = PLCTAG_ERR_REMOTE_ERR;
break;
}
if(pccc->general_status != AB_EIP_OK) {
pdebug(debug,"PCCC command failed, response code: %d",pccc->general_status);
rc = PLCTAG_ERR_REMOTE_ERR;
break;
}
if(pccc->pccc_status != AB_EIP_OK) {
/*pdebug(PLC_LOG_ERR,PLC_ERR_READ, "PCCC command failed, response code: %d",pccc->pccc_status);*/
pdebug(debug,pccc_decode_error(pccc->pccc_data[0]));
rc = PLCTAG_ERR_REMOTE_ERR;
break;
}
tag->status = PLCTAG_STATUS_OK;
rc = PLCTAG_STATUS_OK;
} while(0);
/* let the IO thread free the memory. */
ab_tag_abort(tag);
tag->status = rc;
pdebug(debug,"Done.");
/* Success! */
return rc;
}
int build_read_request(ab_tag_p tag, int slot, int byte_offset)
{
eip_cip_uc_req *cip;
uint8_t *data;
uint8_t *embed_start, *embed_end;
ab_request_p req = NULL;
int debug = tag->debug;
int rc;
pdebug(debug,"Starting.");
/* get a request buffer */
rc = request_create(&req);
if(rc != PLCTAG_STATUS_OK) {
pdebug(debug,"Unable to get new request. rc=%d",rc);
tag->status = rc;
return rc;
}
req->debug = debug;
/* point the request struct at the buffer */
cip = (eip_cip_uc_req*)(req->data);
/* point to the end of the struct */
data = (req->data) + sizeof(eip_cip_uc_req);
/*
* set up the embedded CIP read packet
* The format is:
*
* uint8_t cmd
* LLA formatted name
* uint16_t # of elements to read
*/
embed_start = data;
/* set up the CIP Read request */
*data = AB_EIP_CMD_CIP_READ_FRAG;
data++;
/* copy the tag name into the request */
mem_copy(data,tag->encoded_name,tag->encoded_name_size);
data += tag->encoded_name_size;
/* add the count of elements to read. */
*((uint16_t*)data) = h2le16(tag->elem_count);
data += sizeof(uint16_t);
/* add the byte offset for this request */
*((uint32_t*)data) = h2le32(byte_offset);
data += sizeof(uint32_t);
/* mark the end of the embedded packet */
embed_end = data;
/* Now copy in the routing information for the embedded message */
/*
* routing information. Format:
*
* uint8_t path_size in 16-bit words
* uint8_t reserved/pad (zero)
* uint8_t[...] path (padded to even number of bytes)
*/
*data = (tag->conn_path_size)/2; /* in 16-bit words */
data++;
*data = 0; /* reserved/pad */
data++;
mem_copy(data, tag->conn_path, tag->conn_path_size);
data += tag->conn_path_size;
/* now we go back and fill in the fields of the static part */
/* encap fields */
cip->encap_command = h2le16(AB_EIP_READ_RR_DATA); /* ALWAYS 0x0070 Unconnected Send*/
/* router timeout */
cip->router_timeout = h2le16(1); /* one second timeout, enough? */
/* Common Packet Format fields for unconnected send. */
cip->cpf_item_count = h2le16(2); /* ALWAYS 2 */
cip->cpf_nai_item_type = h2le16(AB_EIP_ITEM_NAI); /* ALWAYS 0 */
cip->cpf_nai_item_length = h2le16(0); /* ALWAYS 0 */
cip->cpf_udi_item_type = h2le16(AB_EIP_ITEM_UDI); /* ALWAYS 0x00B2 - Unconnected Data Item */
cip->cpf_udi_item_length = h2le16(data - (uint8_t*)(&(cip->cm_service_code))); /* REQ: fill in with length of remaining data. */
/* CM Service Request - Connection Manager */
cip->cm_service_code = AB_EIP_CMD_UNCONNECTED_SEND; /* 0x52 Unconnected Send */
cip->cm_req_path_size = 2; /* 2, size in 16-bit words of path, next field */
cip->cm_req_path[0] = 0x20; /* class */
cip->cm_req_path[1] = 0x06; /* Connection Manager */
cip->cm_req_path[2] = 0x24; /* instance */
cip->cm_req_path[3] = 0x01; /* instance 1 */
/* Unconnected send needs timeout information */
cip->secs_per_tick = AB_EIP_SECS_PER_TICK; /* seconds per tick */
cip->timeout_ticks = AB_EIP_TIMEOUT_TICKS; /* timeout = src_secs_per_tick * src_timeout_ticks */
/* size of embedded packet */
cip->uc_cmd_length = h2le16(embed_end - embed_start);
/* set the size of the request */
req->request_size = data - (req->data);
/* mark it as ready to send */
req->send_request = 1;
/* add the request to the session's list. */
rc = request_add(tag->session, req);
if(rc != PLCTAG_STATUS_OK) {
pdebug(debug,"Unable to add request to session! rc=%d",rc);
request_destroy(&req);
tag->status = rc;
return rc;
}
/* save the request for later */
tag->reqs[slot] = req;
pdebug(debug,"Done");
return PLCTAG_STATUS_OK;
}
static int check_read_status(ab_tag_p tag)
{
int rc = PLCTAG_STATUS_OK;
eip_cip_uc_resp *cip_resp;
uint8_t *data;
uint8_t *data_end;
int i;
ab_request_p req;
int byte_offset = 0;
int debug = tag->debug;
/* is there an outstanding request? */
if(!tag->reqs) {
tag->read_in_progress = 0;
tag->status = PLCTAG_ERR_NULL_PTR;
return PLCTAG_ERR_NULL_PTR;
}
for(i = 0; i < tag->num_read_requests; i++) {
if(tag->reqs[i] && !tag->reqs[i]->resp_received) {
tag->status = PLCTAG_STATUS_PENDING;
return PLCTAG_STATUS_PENDING;
}
}
/*
* process each request. If there is more than one request, then
* we need to make sure that we copy the data into the right part
* of the tag's data buffer.
*/
for(i = 0; i < tag->num_read_requests; i++) {
req = tag->reqs[i];
if(!req) {
rc = PLCTAG_ERR_NULL_PTR;
break;
}
/* skip if already processed */
if(req->processed) {
byte_offset += tag->read_req_sizes[i];
continue;
}
req->processed = 1;
pdebug(debug,"processing request %d",i);
/* point to the data */
cip_resp = (eip_cip_uc_resp*)(req->data);
/* point to the start of the data */
data = (req->data)+sizeof(eip_cip_uc_resp);
/* point the end of the data */
data_end = (req->data + cip_resp->encap_length + sizeof(eip_encap_t));
/* check the status */
if(le2h16(cip_resp->encap_command) != AB_EIP_READ_RR_DATA) {
pdebug(debug,"Unexpected EIP packet type received: %d!",cip_resp->encap_command);
rc = PLCTAG_ERR_BAD_DATA;
break;
}
if(le2h16(cip_resp->encap_status) != AB_EIP_OK) {
pdebug(debug,"EIP command failed, response code: %d",cip_resp->encap_status);
rc = PLCTAG_ERR_REMOTE_ERR;
break;
}
if(cip_resp->reply_service != (AB_EIP_CMD_CIP_READ_FRAG | AB_EIP_CMD_CIP_OK)) {
pdebug(debug,"CIP response reply service unexpected: %d",cip_resp->reply_service);
rc = PLCTAG_ERR_BAD_DATA;
break;
}
if(cip_resp->status != AB_CIP_STATUS_OK && cip_resp->status != AB_CIP_STATUS_FRAG) {
pdebug(debug,"CIP read failed with status: %d",cip_resp->status);
pdebug(debug,cip_decode_status(cip_resp->status));
switch(cip_resp->status) {
case 0x04: /* FIXME - should be defined constants */
case 0x05:
case 0x13:
case 0x1C:
rc = PLCTAG_ERR_BAD_PARAM;
break;
default:
rc = PLCTAG_ERR_REMOTE_ERR;
break;
}
break;
}
/* the first byte of the response is a type byte. */
pdebug(debug,"type byte = %d (%x)",(int)*data,(int)*data);
/*
* AB has a relatively complicated scheme for data typing. The type is
* required when writing. Most of the types are basic types and occupy
* a known amount of space. Aggregate types like structs and arrays
* occupy a variable amount of space. In addition, structs and arrays
* can be in two forms: full and abbreviated. Full form for structs includes
* all data types (in full) for fields of the struct. Abbreviated form for
* structs includes a two byte CRC calculated across the full form. For arrays,
* full form includes index limits and base data type. Abbreviated arrays
* drop the limits and encode any structs as abbreviate structs. At least
* we think this is what is happening.
*
* Luckily, we do not actually care what these bytes mean, we just need
* to copy them and skip past them for the actual data.
*/
/* check for a simple/base type */
if((*data) >= AB_CIP_DATA_BIT && (*data) <= AB_CIP_DATA_STRINGI) {
/* copy the type info for later. */
if(tag->encoded_type_info_size == 0) {
tag->encoded_type_info_size = 2;
mem_copy(tag->encoded_type_info,data,tag->encoded_type_info_size);
}
/* skip the type byte and zero length byte */
data += 2;
} else if( (*data) == AB_CIP_DATA_ABREV_STRUCT
|| (*data) == AB_CIP_DATA_ABREV_ARRAY
|| (*data) == AB_CIP_DATA_FULL_STRUCT
|| (*data) == AB_CIP_DATA_FULL_ARRAY)
{
/* this is an aggregate type of some sort, the type info is variable length */
int type_length = *(data+1) + 2; /*
* MAGIC
* add 2 to get the total length including
* the type byte and the length byte.
*/
/* check for extra long types */
if(type_length > MAX_TAG_TYPE_INFO) {
pdebug(debug,"Read data type info is too long (%d)!", type_length);
rc = PLCTAG_ERR_TOO_LONG;
break;
}
/* copy the type info for later. */
if(tag->encoded_type_info_size == 0) {
tag->encoded_type_info_size = type_length;
mem_copy(tag->encoded_type_info,data,tag->encoded_type_info_size);
}
data += type_length;
} else {
pdebug(debug,"Unsupported data type returned, type byte=%d",*data);
rc = PLCTAG_ERR_UNSUPPORTED;
break;
}
/* copy data into the tag. */
if((byte_offset + (data_end - data)) > tag->size) {
pdebug(debug,"Read data is too long (%d bytes) to fit in tag data buffer (%d bytes)!",byte_offset + (int)(data_end-data),tag->size);
rc = PLCTAG_ERR_TOO_LONG;
break;
}
pdebug(debug,"Got %d bytes of data", (data_end - data));
/*
* copy the data, but only if this is not
* a pre-read for a subsequent write! We do not
* want to overwrite the data the upstream has
* put into the tag's data buffer.
*/
if(!tag->pre_write_read) {
mem_copy(tag->data + byte_offset, data, (data_end - data));
}
/* save the size of the response for next time */
tag->read_req_sizes[i] = (data_end - data);
/*
* did we get any data back? a zero-length response is
* an error here.
*/
if((data_end - data) == 0) {
rc = PLCTAG_ERR_NO_DATA;
break;
} else {
/* bump the byte offset */
byte_offset += (data_end - data);
/* set the return code */
rc = PLCTAG_STATUS_OK;
}
} /* end of for(i = 0; i < tag->num_requests; i++) */
/* are we actually done? */
if(rc == PLCTAG_STATUS_OK) {
if(byte_offset < tag->size) {
/* no, not yet */
if(tag->first_read) {
/* call read start again to get the next piece */
pdebug(debug,"calling ab_rag_read_cip_start_unsafe() to get the next chunk.");
rc = eip_cip_tag_read_start(tag);
} else {
pdebug(debug,"Insufficient data read for tag!");
ab_tag_abort(tag);
rc = PLCTAG_ERR_READ;
}
} else {
/* done! */
tag->first_read = 0;
tag->read_in_progress = 0;
/* have the IO thread take care of the request buffers */
ab_tag_abort(tag);
/* Now remove the requests from the session's request list. */
/* FIXME - this functionality has been removed to simplify
* this case. All deallocation of requests is done by the
* IO thread now. The abort call above handles this.
*/
//for(i = 0; i < tag->num_read_requests; i++) {
//int tmp_rc;
//req = tag->reqs[i];
//tmp_rc = request_remove(tag->session, req);
//if(tmp_rc != PLCTAG_STATUS_OK) {
// pdebug(debug,"Unable to remove the request from the list! rc=%d",rc);
//
// /* since we could not remove it, maybe the thread can. */
// req->abort_request = 1;
//
// rc = tmp_rc;
//} else {
// /* free up the request resources */
// request_destroy(&req);
//}
///* mark it as freed */
//tag->reqs[i] = NULL;
//}
/* if this is a pre-read for a write, then pass off the the write routine */
if(tag->pre_write_read) {
pdebug(debug,"Restarting write call now.");
tag->pre_write_read = 0;
rc = eip_cip_tag_write_start(tag);
}
}
} else {
/* error ! */
pdebug(debug,"Error received!");
}
tag->status = rc;
pdebug(debug,"Done.");
return rc;
}
static void parallel_ioport_write_hw(void *opaque, uint32_t addr, uint32_t val)
{
ParallelState *s = opaque;
uint8_t parm = val;
int dir;
/* Sometimes programs do several writes for timing purposes on old
HW. Take care not to waste time on writes that do nothing. */
s->last_read_offset = ~0U;
addr &= 7;
switch(addr) {
case PARA_REG_DATA:
if (s->dataw == val)
return;
pdebug("wd%02x\n", val);
qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_WRITE_DATA, &parm);
s->dataw = val;
break;
case PARA_REG_STS:
pdebug("ws%02x\n", val);
if (val & PARA_STS_TMOUT)
s->epp_timeout = 0;
break;
case PARA_REG_CTR:
val |= 0xc0;
if (s->control == val)
return;
pdebug("wc%02x\n", val);
if ((val & PARA_CTR_DIR) != (s->control & PARA_CTR_DIR)) {
if (val & PARA_CTR_DIR) {
dir = 1;
} else {
dir = 0;
}
qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_DATA_DIR, &dir);
parm &= ~PARA_CTR_DIR;
}
qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_WRITE_CONTROL, &parm);
s->control = val;
break;
case PARA_REG_EPP_ADDR:
if ((s->control & (PARA_CTR_DIR|PARA_CTR_SIGNAL)) != PARA_CTR_INIT)
/* Controls not correct for EPP address cycle, so do nothing */
pdebug("wa%02x s\n", val);
else {
struct ParallelIOArg ioarg = { .buffer = &parm, .count = 1 };
if (qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_EPP_WRITE_ADDR, &ioarg)) {
s->epp_timeout = 1;
pdebug("wa%02x t\n", val);
}
else
pdebug("wa%02x\n", val);
}
break;
case PARA_REG_EPP_DATA:
if ((s->control & (PARA_CTR_DIR|PARA_CTR_SIGNAL)) != PARA_CTR_INIT)
/* Controls not correct for EPP data cycle, so do nothing */
pdebug("we%02x s\n", val);
else {
struct ParallelIOArg ioarg = { .buffer = &parm, .count = 1 };
if (qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_EPP_WRITE, &ioarg)) {
s->epp_timeout = 1;
pdebug("we%02x t\n", val);
}
else
pdebug("we%02x\n", val);
}
break;
}
}
static void
parallel_ioport_eppdata_write_hw2(void *opaque, uint32_t addr, uint32_t val)
{
ParallelState *s = opaque;
uint16_t eppdata = cpu_to_le16(val);
int err;
struct ParallelIOArg ioarg = {
.buffer = &eppdata, .count = sizeof(eppdata)
};
if ((s->control & (PARA_CTR_DIR|PARA_CTR_SIGNAL)) != PARA_CTR_INIT) {
/* Controls not correct for EPP data cycle, so do nothing */
pdebug("we%04x s\n", val);
return;
}
err = qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_EPP_WRITE, &ioarg);
if (err) {
s->epp_timeout = 1;
pdebug("we%04x t\n", val);
}
else
pdebug("we%04x\n", val);
}
static void
parallel_ioport_eppdata_write_hw4(void *opaque, uint32_t addr, uint32_t val)
{
ParallelState *s = opaque;
uint32_t eppdata = cpu_to_le32(val);
int err;
struct ParallelIOArg ioarg = {
.buffer = &eppdata, .count = sizeof(eppdata)
};
if ((s->control & (PARA_CTR_DIR|PARA_CTR_SIGNAL)) != PARA_CTR_INIT) {
/* Controls not correct for EPP data cycle, so do nothing */
pdebug("we%08x s\n", val);
return;
}
err = qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_EPP_WRITE, &ioarg);
if (err) {
s->epp_timeout = 1;
pdebug("we%08x t\n", val);
}
else
pdebug("we%08x\n", val);
}
static uint32_t parallel_ioport_read_sw(void *opaque, uint32_t addr)
{
ParallelState *s = opaque;
uint32_t ret = 0xff;
addr &= 7;
switch(addr) {
case PARA_REG_DATA:
if (s->control & PARA_CTR_DIR)
ret = s->datar;
else
ret = s->dataw;
break;
case PARA_REG_STS:
ret = s->status;
s->irq_pending = 0;
if ((s->status & PARA_STS_BUSY) == 0 && (s->control & PARA_CTR_STROBE) == 0) {
/* XXX Fixme: wait 5 microseconds */
if (s->status & PARA_STS_ACK)
s->status &= ~PARA_STS_ACK;
else {
/* XXX Fixme: wait 5 microseconds */
s->status |= PARA_STS_ACK;
s->status |= PARA_STS_BUSY;
}
}
parallel_update_irq(s);
break;
case PARA_REG_CTR:
ret = s->control;
break;
}
pdebug("read addr=0x%02x val=0x%02x\n", addr, ret);
return ret;
}
static uint32_t parallel_ioport_read_hw(void *opaque, uint32_t addr)
{
ParallelState *s = opaque;
uint8_t ret = 0xff;
addr &= 7;
switch(addr) {
case PARA_REG_DATA:
qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_READ_DATA, &ret);
if (s->last_read_offset != addr || s->datar != ret)
pdebug("rd%02x\n", ret);
s->datar = ret;
break;
case PARA_REG_STS:
qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_READ_STATUS, &ret);
ret &= ~PARA_STS_TMOUT;
if (s->epp_timeout)
ret |= PARA_STS_TMOUT;
if (s->last_read_offset != addr || s->status != ret)
pdebug("rs%02x\n", ret);
s->status = ret;
break;
case PARA_REG_CTR:
/* s->control has some bits fixed to 1. It is zero only when
it has not been yet written to. */
if (s->control == 0) {
qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_READ_CONTROL, &ret);
if (s->last_read_offset != addr)
pdebug("rc%02x\n", ret);
s->control = ret;
}
else {
ret = s->control;
if (s->last_read_offset != addr)
pdebug("rc%02x\n", ret);
}
break;
case PARA_REG_EPP_ADDR:
if ((s->control & (PARA_CTR_DIR|PARA_CTR_SIGNAL)) != (PARA_CTR_DIR|PARA_CTR_INIT))
/* Controls not correct for EPP addr cycle, so do nothing */
pdebug("ra%02x s\n", ret);
else {
struct ParallelIOArg ioarg = { .buffer = &ret, .count = 1 };
if (qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_EPP_READ_ADDR, &ioarg)) {
s->epp_timeout = 1;
pdebug("ra%02x t\n", ret);
}
else
pdebug("ra%02x\n", ret);
}
break;
case PARA_REG_EPP_DATA:
if ((s->control & (PARA_CTR_DIR|PARA_CTR_SIGNAL)) != (PARA_CTR_DIR|PARA_CTR_INIT))
/* Controls not correct for EPP data cycle, so do nothing */
pdebug("re%02x s\n", ret);
else {
struct ParallelIOArg ioarg = { .buffer = &ret, .count = 1 };
if (qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_EPP_READ, &ioarg)) {
s->epp_timeout = 1;
pdebug("re%02x t\n", ret);
}
else
pdebug("re%02x\n", ret);
}
break;
}
s->last_read_offset = addr;
return ret;
}
static uint32_t
parallel_ioport_eppdata_read_hw2(void *opaque, uint32_t addr)
{
ParallelState *s = opaque;
uint32_t ret;
uint16_t eppdata = ~0;
int err;
struct ParallelIOArg ioarg = {
.buffer = &eppdata, .count = sizeof(eppdata)
};
if ((s->control & (PARA_CTR_DIR|PARA_CTR_SIGNAL)) != (PARA_CTR_DIR|PARA_CTR_INIT)) {
/* Controls not correct for EPP data cycle, so do nothing */
pdebug("re%04x s\n", eppdata);
return eppdata;
}
err = qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_EPP_READ, &ioarg);
ret = le16_to_cpu(eppdata);
if (err) {
s->epp_timeout = 1;
pdebug("re%04x t\n", ret);
}
else
pdebug("re%04x\n", ret);
return ret;
}
static uint32_t
parallel_ioport_eppdata_read_hw4(void *opaque, uint32_t addr)
{
ParallelState *s = opaque;
uint32_t ret;
uint32_t eppdata = ~0U;
int err;
struct ParallelIOArg ioarg = {
.buffer = &eppdata, .count = sizeof(eppdata)
};
if ((s->control & (PARA_CTR_DIR|PARA_CTR_SIGNAL)) != (PARA_CTR_DIR|PARA_CTR_INIT)) {
/* Controls not correct for EPP data cycle, so do nothing */
pdebug("re%08x s\n", eppdata);
return eppdata;
}
err = qemu_chr_fe_ioctl(s->chr, CHR_IOCTL_PP_EPP_READ, &ioarg);
ret = le32_to_cpu(eppdata);
if (err) {
s->epp_timeout = 1;
pdebug("re%08x t\n", ret);
}
else
pdebug("re%08x\n", ret);
return ret;
}
static void parallel_ioport_ecp_write(void *opaque, uint32_t addr, uint32_t val)
{
pdebug("wecp%d=%02x\n", addr & 7, val);
}
static uint32_t parallel_ioport_ecp_read(void *opaque, uint32_t addr)
{
uint8_t ret = 0xff;
pdebug("recp%d:%02x\n", addr & 7, ret);
return ret;
}
int eip_cip_tag_read_start(ab_tag_p tag)
{
int rc = PLCTAG_STATUS_OK;
int i;
int byte_offset = 0;
int debug = tag->debug;
pdebug(debug,"Starting");
/* is this the first read? */
if(tag->first_read) {
/*
* On a new tag, the first time we read, we go through and
* request the maximum possible (up to the size of the tag)
* each time. We record what we actually get back in the
* tag->read_req_sizes array. The next time we read, we
* use that array to make the new requests.
*/
rc = allocate_read_request_slot(tag);
if(rc != PLCTAG_STATUS_OK) {
tag->status = rc;
return rc;
}
/*
* The PLC may not send back as much data as we would like.
* So, we attempt to determine what the size will be by
* single-stepping through the requests the first time.
* This will be slow, but subsequent reads will be pipelined.
*/
/* determine the byte offset this time. */
byte_offset = 0;
/* scan and add the byte offsets */
for(i=0; i < tag->num_read_requests && tag->reqs[i]; i++) {
byte_offset += tag->read_req_sizes[i];
}
pdebug(debug,"First read tag->num_read_requests=%d, byte_offset=%d.",tag->num_read_requests,byte_offset);
/* i is the index of the first new request */
rc = build_read_request(tag, i, byte_offset);
if(rc != PLCTAG_STATUS_OK) {
tag->status = rc;
return rc;
}
} else {
/* this is not the first read, so just set up all the requests at once. */
byte_offset = 0;
for(i=0; i < tag->num_read_requests; i++) {
rc = build_read_request(tag, i, byte_offset);
if(rc != PLCTAG_STATUS_OK) {
tag->status = rc;
return rc;
}
byte_offset += tag->read_req_sizes[i];
}
}
/* mark the tag read in progress */
tag->read_in_progress = 1;
/* the read is now pending */
tag->status = PLCTAG_STATUS_PENDING;
pdebug(debug,"Done.");
return PLCTAG_STATUS_PENDING;
}
static void parallel_ioport_write_hw(void *opaque, uint32_t addr, uint32_t val)
{
ParallelState *s = (ParallelState *)opaque;
uint8_t parm = val;
/* Sometimes programs do several writes for timing purposes on old
HW. Take care not to waste time on writes that do nothing. */
s->last_read_offset = ~0U;
addr &= 7;
switch(addr) {
case PARA_REG_DATA:
if (s->dataw == val)
return;
pdebug("wd%02x\n", val);
qemu_chr_ioctl(s->chr, CHR_IOCTL_PP_WRITE_DATA, &parm);
s->dataw = val;
break;
case PARA_REG_STS:
pdebug("ws%02x\n", val);
if (val & PARA_STS_TMOUT)
s->epp_timeout = 0;
break;
case PARA_REG_CTR:
val |= 0xc0;
if (s->control == val)
return;
pdebug("wc%02x\n", val);
qemu_chr_ioctl(s->chr, CHR_IOCTL_PP_WRITE_CONTROL, &parm);
s->control = val;
break;
case PARA_REG_EPP_ADDR:
if ((s->control & (PARA_CTR_DIR|PARA_CTR_SIGNAL)) != PARA_CTR_INIT)
/* Controls not correct for EPP address cycle, so do nothing */
pdebug("wa%02x s\n", val);
else {
struct ParallelIOArg ioarg = { buffer : &parm, count : 1 };
if (qemu_chr_ioctl(s->chr, CHR_IOCTL_PP_EPP_WRITE_ADDR, &ioarg)) {
s->epp_timeout = 1;
pdebug("wa%02x t\n", val);
}
else
pdebug("wa%02x\n", val);
}
break;
case PARA_REG_EPP_DATA:
if ((s->control & (PARA_CTR_DIR|PARA_CTR_SIGNAL)) != PARA_CTR_INIT)
/* Controls not correct for EPP data cycle, so do nothing */
pdebug("we%02x s\n", val);
else {
struct ParallelIOArg ioarg = { buffer : &parm, count : 1 };
if (qemu_chr_ioctl(s->chr, CHR_IOCTL_PP_EPP_WRITE, &ioarg)) {
s->epp_timeout = 1;
pdebug("we%02x t\n", val);
}
else
pdebug("we%02x\n", val);
}
break;
}
}
static uint32_t parallel_ioport_read_hw(void *opaque, uint32_t addr)
{
ParallelState *s = (ParallelState *)opaque;
uint8_t ret = 0xff;
addr &= 7;
switch(addr) {
case PARA_REG_DATA:
qemu_chr_ioctl(s->chr, CHR_IOCTL_PP_READ_DATA, &ret);
if (s->last_read_offset != addr || s->datar != ret)
pdebug("rd%02x\n", ret);
s->datar = ret;
break;
case PARA_REG_STS:
qemu_chr_ioctl(s->chr, CHR_IOCTL_PP_READ_STATUS, &ret);
ret &= ~PARA_STS_TMOUT;
if (s->epp_timeout)
ret |= PARA_STS_TMOUT;
if (s->last_read_offset != addr || s->status != ret)
pdebug("rs%02x\n", ret);
s->status = ret;
break;
case PARA_REG_CTR:
/* s->control has some bits fixed to 1. It is zero only when
it has not been yet written to. */
if (s->control == 0) {
qemu_chr_ioctl(s->chr, CHR_IOCTL_PP_READ_CONTROL, &ret);
if (s->last_read_offset != addr)
pdebug("rc%02x\n", ret);
s->control = ret;
}
else {
ret = s->control;
if (s->last_read_offset != addr)
pdebug("rc%02x\n", ret);
}
break;
case PARA_REG_EPP_ADDR:
if ((s->control & (PARA_CTR_DIR|PARA_CTR_SIGNAL)) != (PARA_CTR_DIR|PARA_CTR_INIT))
/* Controls not correct for EPP addr cycle, so do nothing */
pdebug("ra%02x s\n", ret);
else {
struct ParallelIOArg ioarg = { buffer : &ret, count : 1 };
if (qemu_chr_ioctl(s->chr, CHR_IOCTL_PP_EPP_READ_ADDR, &ioarg)) {
s->epp_timeout = 1;
pdebug("ra%02x t\n", ret);
}
else
pdebug("ra%02x\n", ret);
}
break;
case PARA_REG_EPP_DATA:
if ((s->control & (PARA_CTR_DIR|PARA_CTR_SIGNAL)) != (PARA_CTR_DIR|PARA_CTR_INIT))
/* Controls not correct for EPP data cycle, so do nothing */
pdebug("re%02x s\n", ret);
else {
struct ParallelIOArg ioarg = { buffer : &ret, count : 1 };
if (qemu_chr_ioctl(s->chr, CHR_IOCTL_PP_EPP_READ, &ioarg)) {
s->epp_timeout = 1;
pdebug("re%02x t\n", ret);
}
else
pdebug("re%02x\n", ret);
}
break;
}
s->last_read_offset = addr;
return ret;
}
//int cip_encode_path(ab_tag_p tag, const char *path)
int cip_encode_path(const char *path, int needs_connection, int plc_type, uint8_t **conn_path, uint8_t *conn_path_size, uint16_t *dhp_dest)
{
int ioi_size=0;
int last_is_dhp=0;
int has_dhp=0;
int dhp_channel=0;
int src_addr=0;
int dest_addr=0;
int tmp=0;
char **links=NULL;
char *link=NULL;
uint8_t tmp_path[MAX_CONN_PATH+16];
uint8_t *data = &tmp_path[0];
/* split the path */
if(path) {
links = str_split(path,",");
}
if(links != NULL) {
int link_index=0;
/* work along each string. */
link = links[link_index];
while(link && ioi_size < MAX_CONN_PATH) { /* MAGIC -2 to allow for padding */
int rc = match_dhp_node(link,&dhp_channel,&src_addr,&dest_addr);
if(rc > 0) {
/* we matched a DH+ route node */
pdebug(DEBUG_DETAIL,"Found DH+ routing, need connection. Conn path length=%d",ioi_size);
last_is_dhp = 1;
has_dhp = 1;
} else if (rc < 0) {
/* matched part of a DH+ node, but then failed. Syntax error. */
pdebug(DEBUG_WARN, "Syntax error in DH+ route path.");
if(links) mem_free(links);
return PLCTAG_ERR_BAD_PARAM;
} else {
/* did not match a DH+ route node, but no error. */
last_is_dhp = 0;
has_dhp = 0;
if(str_to_int(link, &tmp) != 0) {
/* syntax error */
pdebug(DEBUG_WARN, "Syntax error in path, expected number!");
if(links) mem_free(links);
return PLCTAG_ERR_BAD_PARAM;
}
*data = (uint8_t)tmp;
/*printf("convert_links() link(%d)=%s (%d)\n",i,*links,tmp);*/
data++;
ioi_size++;
pdebug(DEBUG_DETAIL,"Found regular routing. Conn path length=%d",ioi_size);
}
/* FIXME - handle case where IP address is in path */
link_index++;
link = links[link_index];
}
/* we do not need the split string anymore. */
if(links) {
mem_free(links);
links = NULL;
}
}
/* Add to the path based on the protocol type and
* whether the last part is DH+. Only some combinations of
* DH+ and PLC type work.
*/
if(last_is_dhp && plc_type == AB_PROTOCOL_PLC) {
/* We have to make the difference from the more
* generic case.
*/
/* try adding this onto the end of the routing path */
*data = 0x20;
data++;
*data = 0xA6;
data++;
*data = 0x24;
data++;
*data = (uint8_t)dhp_channel;
data++;
*data = 0x2C;
data++;
*data = 0x01;
data++;
ioi_size += 6;
*dhp_dest = (uint16_t)dest_addr;
} else if(!has_dhp) {
if(needs_connection) {
/*
* we do a generic path to the router
* object in the PLC. But only if the PLC is
* one that needs a connection. For instance a
* Micro850 needs to work in connected mode.
*/
*data = 0x20; /* class */
data++;
*data = 0x02; /* message router class */
data++;
*data = 0x24; /* instance */
data++;
*data = 0x01; /* message router class instance #1 */
ioi_size += 4;
}
*dhp_dest = 0;
} else {
/* we had the special DH+ format and it was
* either not last or not a PLC5/SLC. That
* is an error.
*/
*dhp_dest = 0;
return PLCTAG_ERR_BAD_PARAM;
}
/*
* zero out the last byte if we need to.
* This pads out the path to a multiple of 16-bit
* words.
*/
pdebug(DEBUG_DETAIL,"ioi_size before %d", ioi_size);
if(ioi_size & 0x01) {
*data = 0;
ioi_size++;
}
/* allocate space for the connection path */
*conn_path = mem_alloc(ioi_size);
if(! *conn_path) {
pdebug(DEBUG_WARN, "Unable to allocate connection path!");
return PLCTAG_ERR_NO_MEM;
}
mem_copy(*conn_path, &tmp_path[0], ioi_size);
*conn_path_size = (uint8_t)ioi_size;
pdebug(DEBUG_INFO, "Done.");
return PLCTAG_STATUS_OK;
}
int io_devctl (resmgr_context_t *ctp, io_devctl_t *msg, RESMGR_OCB_T *ocb){
int nbytes, status, previous;
uint32_t chip, page, reg;
uint32_t value, temp, command, offset;
uint32_t sequencer_base = 0x00100000;
struct ICS660_FILTER filter_str;
struct ICS660_FREQ freq_str;
struct ICS660_PHASE phase_str;
struct ICS660_sequencer seq_data;
union {
data_t data;
uint32_t data32;
int int_data;
struct ICS660_FILTER filter_str;
struct ICS660_FREQ freq_str;
struct ICS660_PHASE phase_str;
struct dc60m_p_val dc60m_p;
struct dc60m_pg_reg pg_reg;
struct ICS660_sequencer seq_data;
} *rx_data;
pdebug("IO_DEVCTL - entry -\n");
if((status = iofunc_devctl_default(ctp, msg, ocb)) != _RESMGR_DEFAULT)
return(status);
rx_data = _DEVCTL_DATA(msg->i);
//pdebug("IO_DEVCTL - rx_data %d filter_str %d\n",rx_data,sizeof(rx_data->filter_str));
//pdebug("IO_DEVCTL - msg %d msg.i %d \n",msg,msg->i);
command = get_device_command(msg->i.dcmd);
pdebug("IO_DEVCTL - command %x %x\n",msg->i.dcmd,command);
switch(msg->i.dcmd){
case ICS660_BOARD_RESET:
pdebug("IO_DEVCTL - BOARD_RESET %x\n",value);
*(unsigned int*)(ics660->regs.board_reset) = RESET_VALUE;
break;
case ICS660_CLEAR_IMASK:
pdebug("IO_DEVCTL - CLEAR_IMASK %x\n",value);
*(unsigned int*)(ics660->regs.interrupt_mask) = (uint32_t)IMASK_VALUE;
break;
case ICS660_TRIGGER_SOURCE:
value = rx_data->data32;
pdebug("IO_DEVCTL - TRIGGER_SOURCE %x\n",value);
temp = *(uint32_t *)ics660->regs.control & ~TRIGGER_SOURCE;
*(uint32_t *)ics660->regs.control = temp | value;
break;
case ICS660_CLOCK_SELECT:
value = rx_data->data32;
temp = *(uint32_t *)ics660->regs.control & ~CLOCK_SELECT;
value = (value << 1) & CLOCK_SELECT;
pdebug("IO_DEVCTL - CLOCK_SELECT %x\n",value);
*(uint32_t *)ics660->regs.control = temp | value;
break;
case ICS660_MODE:
value = rx_data->data32;
temp = *(uint32_t *)ics660->regs.control & ~MODE;
value = value & MODE;
pdebug("IO_DEVCTL - ICS660_MODE %x\n",value);
*(uint32_t *)ics660->regs.control = temp | value;
break;
case ICS660_UPCONVERTER_SELECT:
value = rx_data->data32;
temp = *(uint32_t *)ics660->regs.control & ~UPCONVERTER_SELECT;
value = (value << 4) & UPCONVERTER_SELECT;
pdebug("IO_DEVCTL - UPCONVERTER_SELECT %x\n",value);
*(uint32_t *)ics660->regs.control = temp | value;
break;
case ICS660_DATA_SOURCE:
value = rx_data->data32;
temp = *(uint32_t *)ics660->regs.control & ~DATA_SOURCE;
value = (value << 5) & DATA_SOURCE;
pdebug("IO_DEVCTL - DATA_SOURCE %x\n",value);
*(uint32_t *)ics660->regs.control = temp | value;
break;
case ICS660_FPDP_CLOCK_SEL:
value = rx_data->data32;
temp = *(uint32_t *)ics660->regs.control & ~FPDP_CLOCK_SEL;
value = (value << 6) & FPDP_CLOCK_SEL;
pdebug("IO_DEVCTL - FPDP_CLOCK_SEL %x\n",value);
*(uint32_t *)ics660->regs.control = temp | value;
break;
case ICS660_FPDP_TERM:
value = rx_data->data32;
temp = *(uint32_t *)ics660->regs.control & ~FPDP_TERM;
value = (value << 7) & FPDP_TERM;
pdebug("IO_DEVCTL - FPDP_TERM %x\n",value);
*(uint32_t *)ics660->regs.control = temp | value;
break;
case ICS660_SYNC_MASTER:
value = rx_data->data32;
temp = *(uint32_t *)ics660->regs.control & ~SYNC_MASTER;
value = (value << 8) & SYNC_MASTER;
pdebug("IO_DEVCTL - SYNC_MASTER %x\n",value);
*(uint32_t *)ics660->regs.control = temp | value;
break;
case ICS660_SYNC_TERM:
value = rx_data->data32;
temp = *(uint32_t *)ics660->regs.control & ~SYNC_TERM;
value = (value << 9) & SYNC_TERM;
pdebug("IO_DEVCTL - SYNC_TERM %x\n",value);
*(uint32_t *)ics660->regs.control = temp | value;
break;
case ICS660_DAC_ENABLE:
value = rx_data->data32;
temp = *(uint32_t *)ics660->regs.control & ~DAC_ENABLE;
value = (value << 10) & DAC_ENABLE;
pdebug("IO_DEVCTL - DAC_ENABLE %x\n",value);
*(uint32_t *)ics660->regs.control = temp | value;
break;
case ICS660_DAC_RESET:
value = rx_data->data32;
pdebug("IO_DEVCTL - DAC_RESET %x\n",value);
*(uint32_t *)ics660->regs.dac_reset = value;
break;
case ICS660_TRIGGER:
value = rx_data->data32;
temp = *(uint32_t *)ics660->regs.control & ~TRIGGER;
value = (value << 11) & TRIGGER;
pdebug("IO_DEVCTL - TRIGGER %x\n",value);
*(uint32_t *)ics660->regs.control = temp | value;
break;
case ICS660_BANK_WIDTH:
value = rx_data->int_data;
pdebug("IO_DEVCTL - BANK_WIDTH %d\n",value);
*(uint32_t *)ics660->regs.bank_width = (uint32_t) value;
break;
case ICS660_BANK_LENGTH:
value = rx_data->int_data;
pdebug("IO_DEVCTL - BANK_LENGTH %d\n",value);
*(uint32_t *)ics660->regs.bank_length = (uint32_t) value;
break;
case ICS660_PERSISTENCE_REG:
value = rx_data->int_data;
pdebug("IO_DEVCTL - PERSISTENCE %d\n",value);
*(uint32_t *)ics660->regs.persistence = (uint32_t) value;
break;
case ICS660_TRANS_LENGTH:
value = rx_data->int_data;
pdebug("IO_DEVCTL - TRANSMIT_LENGTH: %d\n",value);
*(uint32_t *)ics660->regs.trans_length = (uint32_t) value;
break;
case ICS660_WRITE_SEQUENCER:
pdebug("IO_DEVCTL - WRITE_SEQUENCER offset: %x value: %x\n",(uint32_t)rx_data->seq_data.offset,(uint32_t)rx_data->seq_data.value );
seq_data.offset = rx_data->seq_data.offset;
seq_data.value = rx_data->seq_data.value;
*(uint32_t *)(ics660->mem0 + sequencer_base + (uint32_t)seq_data.offset)
= (uint32_t)seq_data.value;
break;
case ICS660_LOAD_FILTER:
pdebug("_ICS660_DRV: LOAD_FILTER\n");
filter_str.chip = rx_data->filter_str.chip;
filter_str.channel = rx_data->filter_str.channel;
filter_str.f_corner = rx_data->filter_str.f_corner;
filter_str.state_time = rx_data->filter_str.state_time;
pdebug("_ICS660_DRV: LOAD_FILTER - \n CHIP %d\n CHANNEL %d\n F_CORNER %d\n STATE_TIME %f\n",filter_str.chip,filter_str.channel,filter_str.f_corner,filter_str.state_time);
select_chip(ics660->mod_control_reg,filter_str.chip);
load_filter_taps(ics660->dc60m_regs,filter_str.channel,filter_str.f_corner,
filter_str.state_time);
select_chip(ics660->mod_control_reg,0);
break;
case ICS660_LOAD_FREQUENCY:
pdebug("_ICS660_DRV: LOAD_FREQUENCY\n");
freq_str.chip = rx_data->freq_str.chip;
freq_str.channel = rx_data->freq_str.channel;
freq_str.freq = rx_data->freq_str.freq;
select_chip(ics660->mod_control_reg,freq_str.chip);
pdebug("_ICS660_DRV: LOAD_FREQ - \n CHIP %d\n CHANNEL %d\n FREQ %f\n",freq_str.chip,freq_str.channel,freq_str.freq);
load_frequency(ics660->dc60m_regs,freq_str.channel,freq_str.freq);
break;
case ICS660_LOAD_PHASE:
pdebug("_ICS660_DRV: LOAD_PHASE\n");
phase_str.chip = rx_data->phase_str.chip;
phase_str.channel = rx_data->phase_str.channel;
phase_str.phase = rx_data->phase_str.phase;
select_chip(ics660->mod_control_reg,phase_str.chip);
load_phase(ics660->dc60m_regs,phase_str.channel,phase_str.phase);
break;
case ICS660_SET_DC_READY:
pdebug("IO_DEVCTL - SET_DC_READY %x\n",value);
set_dc_ready(ics660->mod_control_reg);
break;
case ICS660_RELEASE_RESETS:
pdebug("IO_DEVCTL - RELEASE_RESETS %x\n",value);
release_dc60m_resets(ics660->mod_control_reg,ics660->dc60m_regs);
break;
case ICS660_SET_CHIP:
chip = rx_data->data32;
select_chip(ics660->mod_control_reg,chip);
break;
case ICS660_SET_PAGE:
chip = rx_data->data32;
select_chip(ics660->mod_control_reg,chip);
break;
case ICS660_MODULE_CONTROL_REG:
value = rx_data->data32;
pdebug("IO_DEVCTL - MODULE_CONTROL_REG %d\n",value);
*(uint32_t *)ics660->mod_control_reg = (uint32_t)value;
break;
case ICS660_DC60M_RESET:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - DC60M_RESET - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.reset = (uint32_t) value;
break;
case ICS660_MISCELANEOUS:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - MISCELANEOUS - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.miscelaneous = (uint32_t)value;
break;
case ICS660_SYNC_MODE:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - SYNC_MODE - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.sync_mode = (uint32_t) value;
break;
case ICS660_INTERPOLATION_MODE:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - INTERPOLATION_MODE - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.interpolation_mode = (uint32_t) value;
break;
case ICS660_CHANNEL_A_SYNC:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - CHANNEL_A_SYNC - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.channel_a_sync = (uint32_t) value;
break;
case ICS660_CHANNEL_B_SYNC:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - CHANNEL_B_SYNC - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.channel_b_sync = (uint32_t) value;
break;
case ICS660_CHANNEL_C_SYNC:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - CHANNEL_C_SYNC - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.channel_c_sync = (uint32_t) value;
break;
case ICS660_CHANNEL_D_SYNC:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - CHANNEL_D_SYNC - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.channel_d_sync = (uint32_t) value;
break;
case ICS660_CHANNEL_FLUSH_MODE:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - CHANNEL_FLUSH_MODE - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.channel_flush_mode = (uint32_t) value;
break;
case ICS660_INTERPOLATION_GAIN:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - INTERPOLATION_GAIN - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.interpolation_gain = (uint32_t) value;
break;
case ICS660_INTERPOLATION_BYTE0:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - INTERPOLATION_BYTE0 - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.interpolation_byte0 = (uint32_t) value;
break;
case ICS660_INTERPOLATION_BYTE1:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - INTERPOLATION_BYTE1 - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.interpolation_byte1 = (uint32_t) value;
break;
case ICS660_COUNTER_BYTE0:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - COUNTER_BYTE0 - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.counter_byte0 = (uint32_t) value;
break;
case ICS660_COUNTER_BYTE1:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - COUNTER_BYTE1 - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.counter_byte1 = (uint32_t) value;
break;
case ICS660_DC_STAT:
chip = rx_data->dc60m_p.chip;
value = rx_data->dc60m_p.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - STAT - chip %d value %x\n",chip,value);
*(uint32_t *)ics660->dc60m_regs.stat = (uint32_t) value;
break;
case ICS660_SET_PAGE_REG:
page = rx_data->pg_reg.page;
chip = rx_data->pg_reg.chip;
reg = rx_data->pg_reg.reg;
value = rx_data->pg_reg.value;
select_chip(ics660->mod_control_reg,chip);
pdebug("IO_DEVCTL - PAGE_REG - chip %d page %d reg %d value %x\n",chip,page,reg,value);
*(uint32_t *)ics660->dc60m_regs.page = page;
*(uint32_t *)ics660->dc60m_regs.input_registers[(int)reg] = (uint32_t) value;
break;
default:
pdebug("IO_DEVCTL - ERROR - no matching case %d\n",value);
return(ENOSYS);
}
memset(&msg->o, 0, sizeof(msg->o));
msg->o.ret_val = status;
msg->o.nbytes = nbytes;
return(_RESMGR_PTR(ctp, &msg->o, sizeof(msg->o)+nbytes));
}
void* asyncWrite(void* parameters)
{
pdebug("asyncWrite()\n");
writeParams* params = parameters;
int64_t write = openForWrite(params->temp_file_name);
if(write < 0)
{
pdebug("Error opening file for write\n");
*(params->error) = FILE_IO_FAIL;
return NULL;
}
chunk* write_chunk = NULL;
uint64_t bytes_written = 0;
uint64_t bytes_to_write = 0;
uint64_t write_progress = 0;
while(*(params->running) && *(params->error) == 0)
{
if(write_chunk == NULL) //Get a chunk from the queue
{
pthread_mutex_lock(params->mutex);
if(front(params->in) != NULL)
{
write_chunk = front(params->in);
if(write_chunk != NULL) { deque(params->in); }
}
pthread_mutex_unlock(params->mutex);
}
//check for termination conditions
if((write_chunk != NULL && write_chunk->action == DONE) ||
((params->file_size) != NULL &&
*(params->valid) &&
bytes_written >= *(params->file_size)))
//size constraints
{
pdebug("^^^^ AsyncWrite() terminating loop ^^^^\n");
destroyChunk(write_chunk);
break;
}
if(write_chunk != NULL && write_chunk->data != NULL)
{
//ensure we aren't writing padding or attack attempts
if(params->file_size == NULL) { bytes_to_write = write_chunk->data_size; }
else if(bytes_written + write_chunk->data_size > *(params->file_size))
{ bytes_to_write = *(params->file_size) - bytes_written; }
else { bytes_to_write = write_chunk->data_size; }
write_progress += write_chunk->data_size;
if(!writeBytes((uint8_t*)write_chunk->data, bytes_to_write, write))
{
pdebug("^^^^ File I/O Error ^^^^\n");
*(params->error) = FILE_IO_FAIL;
close(write);
break;
}
///write it to the file
pdebug("^^^^ Writing a chunk of size %lu ^^^^\n", bytes_to_write);
bytes_written += bytes_to_write;
destroyChunk(write_chunk); //free the chunk
write_chunk = NULL;
}
if(write_progress > 0) //update the progress bar
{
if(pthread_mutex_trylock(params->progress->progress_mutex) == 0)
{
params->progress->progress += write_progress;
write_progress = 0;
pthread_mutex_unlock(params->progress->progress_mutex);
}
}
}
close(write);
if(*(params->error) == 0)
{
if(params->args->rename) //change the output file to what the user wanted
{ rename((char*)params->temp_file_name, (char*)params->args->rename_file); }
else //rename the temp file to the original file name (replaces file)
{ rename((char*)params->temp_file_name, (char*)params->args->target_file); }
//signal any other running threads to stop (in case they are stuck)
*(params->running) = false;
}
pdebug("^^^^ writeThread Done ^^^^\n");
//we are done
return NULL;
}
bool handleKeys(arguments* args,
ThreefishKey_t* cipher_context,
MacCtx_t* mac_context)
{
pdebug("handleKeys()\n");
//sanity check
if (args == NULL || cipher_context == NULL || mac_context == NULL)
{ return false; }
const uint64_t block_byte_size = (uint64_t)args->state_size/8;
uint64_t* cipher_key = NULL;
uint64_t* mac_key = NULL;
//no hash password (bad idea)
if (args->hash == false && args->hash_from_file == false)
{
cipher_key = noHashKey(args->password, args->pw_length, args->state_size);
} //no hash from file (bad idea)
else if (args->hash == false && args->hash_from_file == true)
{
cipher_key = noHashBlockFromFile(args->key_file, args->state_size);
} //hash user entered password to be hashed
else if (args->hash == true && args->hash_from_file == false)
{
cipher_key = (uint64_t*)keyHash(args->password,
args->pw_length,
args->state_size);
} //hash user entered password from file
else if (args->hash == true && args->hash_from_file == true)
{
printf("Hashing key from file... ");
cipher_key = hashKeyFromFile(args->key_file, args->state_size);
printf("Done\n");
}
if (cipher_key == NULL) { return false; } //failure
//Generate IV if we are encrypting
if (args->encrypt == true)
{
if (genIV(args) == false) { return false; } //can't continue without an IV
}
else //and get it from the first part of the file if we are decrypting
{
if (getIV(args) == false) { return false; }
}
//stretch the key with scrypt
if (args->hash == true && args->legacy_hash == false)
{
printf("Stretching key this may take a bit... ");
if (kdf_scrypt((uint8_t*)cipher_key, (size_t)(args->state_size/8),
(uint8_t*)args->iv, (size_t)(args->state_size/8),
SCRYPT_N, SCRYPT_R,
SCRYPT_P, (uint8_t*)cipher_key,
(size_t)(args->state_size/8)
) != 0)
{ return false; } //scrypt failed
printf("Done\n");
}
//generate the mac key from the cipher_key
mac_key = (uint64_t*)keyHash((uint8_t*)cipher_key,
block_byte_size,
args->state_size);
//initialize the key structure for the cipher key
threefishSetKey(cipher_context,
(ThreefishSize_t)args->state_size,
cipher_key,
threefizer_tweak);
//initialize the mac context and undelying skein structures
InitMacCtx(args, mac_context, mac_key);
//free allocated resources
if (cipher_key != NULL) { free(cipher_key); }
if (mac_key != NULL) { free(mac_key); }
return true;
}
int main(int argc, const char **argv)
{
hashtable_p table = NULL;
int size = START_CAPACITY;
float best_utilization = 0.0;
float tmp_utilization = 0.0;
(void)argc;
(void)argv;
pdebug(DEBUG_INFO,"Starting hashtable tests.");
set_debug_level(DEBUG_SPEW);
/* create a hashtable */
pdebug(DEBUG_INFO,"Creating hashtable with at least capacity %d.", START_CAPACITY);
table = hashtable_create(START_CAPACITY);
assert(table != NULL);
assert(hashtable_capacity(table) >= START_CAPACITY);
assert(hashtable_entries(table) == 0);
size = hashtable_capacity(table);
/* insert tests. */
for(int i=1; i <= INSERT_ENTRIES; i++) {
int rc = hashtable_put(table, i, (void*)(intptr_t)i);
assert(rc == PLCTAG_STATUS_OK);
if(hashtable_capacity(table) != size) {
pdebug(DEBUG_INFO, "Hashtable expanded from %d entries to %d entries after inserting %d entries.", size, hashtable_capacity(table), hashtable_entries(table));
size = hashtable_capacity(table);
}
}
tmp_utilization = (float)(hashtable_entries(table))/(float)(hashtable_capacity(table));
pdebug(DEBUG_INFO, "Current table utilization %f%%", tmp_utilization*100.0);
if(tmp_utilization > best_utilization) {
best_utilization = tmp_utilization;
}
assert(hashtable_entries(table) == INSERT_ENTRIES);
pdebug(DEBUG_INFO, "Hash table has correct number of used entries, %d.", hashtable_entries(table));
pdebug(DEBUG_INFO, "Hashtable using %d entries of %d entries capacity.", hashtable_entries(table), hashtable_capacity(table));
/* retrieval tests. */
pdebug(DEBUG_INFO, "Running retrieval tests.");
for(int i=INSERT_ENTRIES; i > 0; i--) {
void *res = hashtable_get(table, i);
assert(res != NULL);
assert(i == (int)(intptr_t)res);
}
/* insert + delete tests. */
pdebug(DEBUG_INFO, "Running combined insert and delete tests.");
for(int i=INSERT_ENTRIES+1; i < (INSERT_ENTRIES*2); i++) {
int rc = hashtable_put(table, i, (void*)(intptr_t)i);
void *res = NULL;
assert(rc == PLCTAG_STATUS_OK);
res = hashtable_remove(table, (i - INSERT_ENTRIES));
assert((i - INSERT_ENTRIES) == (int)(intptr_t)res);
}
assert(hashtable_entries(table) == INSERT_ENTRIES);
pdebug(DEBUG_INFO, "Hash table has correct number of used entries, %d.", hashtable_entries(table));
tmp_utilization = (float)(hashtable_entries(table))/(float)(hashtable_capacity(table));
pdebug(DEBUG_INFO, "Current table utilization %f%%", tmp_utilization*100.0);
if(tmp_utilization > best_utilization) {
best_utilization = tmp_utilization;
}
pdebug(DEBUG_INFO, "Best table utilization %f%%", best_utilization*100.0);
hashtable_destroy(table);
pdebug(DEBUG_INFO, "Done.");
}
int WindscreenLocator::histoSegment(double* histo, int size, double ratio, int &a, int &b, double interest)
{
int maxIndex;
histoMaxIndex(histo, size, maxIndex);
segThreshold = ratio * histo[maxIndex];
pdebug("maxIndex=%d\n", maxIndex);
int interval[10][2];
int index = 0;
// memset(interval, 0, sizeof(interval) * sizeof(interval[0]) * sizeof(int));
bool started = false;
int ret = 0;
int start = 0;
int end = 0;
for(int i = 0; i < size; i++){
if(!started){
if(histo[i] > segThreshold){
start = i;
started = true;
}
}else{
if(histo[i] > segThreshold && i != size - 1){
continue;
}
end = i;
started = false;
interval[index][0] = start;
interval[index][1] = end;
index++;
if(index > 9)
break;
}
}
if(index == 0){
a = 0;
b = size - 1;
ret = 0;
}else{
int aveWidth = 0;
for(int i = 0; i < index; i++){
aveWidth += interval[i][1] - interval[i][0];
}
aveWidth /= index;
int plateWidth = max(1, aveWidth);
if(plate->isValid()){
plateWidth = plate->getPlateWidth();
if(reviser && reviser->getPlateWidth() > 0)
plateWidth = reviser->getPlateWidth();
}
int closestMidIndex = 0;
int dist2Mid = size;
bool found = false;
double f = fabs(interest - 0.5) < 0.01 ? 1.0 : 0.65;
for(int i = 0; i < index; i++){
if(interval[i][1] - interval[i][0] < plateWidth * f )
continue;
int mid = (interval[i][1] + interval[i][0]) / 2;
int iDist = abs(mid - size * interest);
if(iDist < dist2Mid){
closestMidIndex = i;
dist2Mid = iDist;
found = true;
}
ret = i + 1;
}
int gap = 0;
if(!found){ // 返回最大的一个
for(int i = 0; i < index; i++){
if(interval[i][1] - interval[i][0] > gap){
closestMidIndex = i;
gap = interval[i][1] - interval[i][0];
}
}
}
a = interval[closestMidIndex][0];
b = interval[closestMidIndex][1];
}
pdebug("ret=%d\n", ret);
return ret;
}
static void parallel_ioport_ecp_write(void *opaque, uint32_t addr, uint32_t val)
{
addr &= 7;
pdebug("wecp%d=%02x\n", addr, val);
}
void WindscreenLocator::examinSegment(int& xa, int& xb)
{
if(!plate->isValid()){
return;
}
int plateWidth = plate->getPlateWidth();
if(reviser && reviser->getPlateWidth() > 0){
plateWidth = reviser->getPlateWidth();
}
int plateMid = plate->getMiddlePoint().x;
if(plateMid <= xa || plateMid >= xb)
return;
double carWidth2Plate = 3;
int borderMid = (xa + xb) / 2;
if(abs(borderMid - plateMid) > plateWidth / 2){
if((xb - xa) < carWidth2Plate * plateWidth){
if(xb - plateMid > plateMid - xa){
xa = plateMid - (xb - plateMid);
}else{
xb = plateMid + (plateMid - xa);
}
}else if((xb - xa) > 6 * plateWidth){
if(xb - plateMid > plateMid - xa){
xb = plateMid + (plateMid - xa);
}else{
xa = plateMid - (xb - plateMid);
}
}else if(plate->color == Yellow){
if(xb - plateMid > plateMid - xa){
int possibleXa = plateMid - (xb - plateMid);
possibleXa = min(imgRGB->width - 1, possibleXa);
possibleXa = max(0, possibleXa);
// if(horizon2X[possibleXa] >= segThreshold - 2){
xa = possibleXa;
// }
}else{
int possibleXb = plateMid + (plateMid - xa);
possibleXb = min(imgRGB->width - 1, possibleXb);
possibleXb = max(0, possibleXb);
pdebug("segthres = %f, val=%f\n", segThreshold, horizon2X[possibleXb]);
// if(horizon2X[possibleXb] >= segThreshold - 2){
xb = possibleXb;
// }
}
}
}
if((xb - xa) < carWidth2Plate * plateWidth){
xa = plateMid - 3 * plateWidth / 2;
xb = plateMid + 3 * plateWidth / 2;
}
if((xb - xa) > 6 * plateWidth){
xa = plateMid - 3 * plateWidth;
xb = plateMid + 3 * plateWidth;
}
xa = max(0, xa);
xb = min(xb, imgGrad->width - 1);
}
plc_tag ab_tag_create(attr attribs)
{
ab_tag_p tag = AB_TAG_NULL;
const char *path;
int rc;
int debug = attr_get_int(attribs,"debug",0);
pdebug(debug,"Starting.");
/*
* allocate memory for the new tag. Do this first so that
* we have a vehicle for returning status.
*/
tag = (ab_tag_p)mem_alloc(sizeof(struct ab_tag_t));
if(!tag) {
pdebug(debug,"Unable to allocate memory for AB EIP tag!");
return PLC_TAG_NULL;
}
/* store the debug status */
tag->debug = debug;
/*
* check the CPU type.
*
* This determines the protocol type.
*/
if(check_cpu(tag, attribs) != PLCTAG_STATUS_OK) {
tag->status = PLCTAG_ERR_BAD_DEVICE;
return (plc_tag)tag;
}
/* AB PLCs are little endian. */
tag->endian = PLCTAG_DATA_LITTLE_ENDIAN;
/* allocate memory for the data */
tag->elem_count = attr_get_int(attribs,"elem_count",1);
tag->elem_size = attr_get_int(attribs,"elem_size",0);
tag->size = (tag->elem_count) * (tag->elem_size);
if(tag->size == 0) {
/* failure! Need data_size! */
tag->status = PLCTAG_ERR_BAD_PARAM;
return (plc_tag)tag;
}
tag->data = (uint8_t*)mem_alloc(tag->size);
if(tag->data == NULL) {
tag->status = PLCTAG_ERR_NO_MEM;
return (plc_tag)tag;
}
/* get the connection path, punt if there is not one and we have a Logix-class PLC. */
path = attr_get_str(attribs,"path",NULL);
if(path == NULL && tag->protocol_type == AB_PROTOCOL_LGX) {
tag->status = PLCTAG_ERR_BAD_PARAM;
return (plc_tag)tag;
}
/* make sure the global mutex is set up */
rc = check_mutex(tag->debug);
if(rc != PLCTAG_STATUS_OK) {
tag->status = rc;
return (plc_tag)tag;
}
tag->first_read = 1;
/*
* now we start the part that might conflict with other threads.
*
* The rest of this is inside a locked block.
*/
pdebug(debug,"Locking mutex");
critical_block(io_thread_mutex) {
/*
* set up tag vtable. This is protocol specific
*/
tag->vtable = set_tag_vtable(tag);
if(!tag->vtable) {
pdebug(debug,"Unable to set tag vtable!");
tag->status = PLCTAG_ERR_BAD_PARAM;
break;
}
/*
* Check the request IO handler thread.
*/
if(!io_handler_thread) {
rc = thread_create((thread_p*)&io_handler_thread,request_handler_func, 32*1024, NULL);
if(rc != PLCTAG_STATUS_OK) {
pdebug(debug,"Unable to create request handler thread!");
tag->status = rc;
break;
}
}
/*
* Find or create a session.
*/
if(find_or_create_session(tag, attribs) != PLCTAG_STATUS_OK) {
pdebug(debug,"Unable to create session!");
tag->status = PLCTAG_ERR_BAD_GATEWAY;
break;
}
/*
* parse the link path into the tag. Note that it must
* pad the byte string to a multiple of 16-bit words. The function
* also adds the protocol/PLC specific routing information to the
* links specified. This fills in fields in the connection about
* any DH+ special data.
*
* Skip this if we don't have a path.
*/
if(path && cip_encode_path(tag,path) != PLCTAG_STATUS_OK) {
pdebug(debug,"Unable to convert links strings to binary path!");
tag->status = PLCTAG_ERR_BAD_PARAM;
break;
}
/*
* check the tag name, this is protocol specific.
*/
if(check_tag_name(tag, attr_get_str(attribs,"name","NONE")) != PLCTAG_STATUS_OK) {
pdebug(debug,"Bad tag name!");
tag->status = PLCTAG_ERR_BAD_PARAM;
break;
}
/*
* add the tag to the session's list.
*/
if(session_add_tag_unsafe(tag, tag->session) != PLCTAG_STATUS_OK) {
pdebug(debug,"unable to add new tag to connection!");
tag->status = PLCTAG_ERR_CREATE;
break;
}
}
pdebug(debug,"Done.");
return (plc_tag)tag;
}
int calculate_write_sizes(ab_tag_p tag)
{
int overhead;
int data_per_packet;
int num_reqs;
int rc = PLCTAG_STATUS_OK;
int i;
int byte_offset;
int debug = tag->debug;
pdebug(debug,"Starting.");
if(tag->num_write_requests > 0) {
pdebug(debug,"Early termination, write sizes already calculated.");
return rc;
}
/* if we are here, then we have all the type data etc. */
overhead = sizeof(eip_cip_uc_req) /* base packet size */
+ 1 /* service request, one byte */
+ tag->encoded_name_size /* full encoded name */
+ tag->encoded_type_info_size /* encoded type size */
+ tag->conn_path_size+2 /* encoded device path size plus two bytes for length and padding */
+ 2 /* element count, 16-bit int */
+ 4 /* byte offset, 32-bit int */
+ 8; /* MAGIC fudge factor */
data_per_packet = MAX_EIP_PACKET_SIZE - overhead;
/* we want a multiple of 4 bytes */
data_per_packet &= 0xFFFFFFFC;
if(data_per_packet <= 0) {
pdebug(debug,"Unable to send request. Packet overhead, %d bytes, is too large for packet, %d bytes!", overhead, MAX_EIP_PACKET_SIZE);
tag->status = PLCTAG_ERR_TOO_LONG;
return PLCTAG_ERR_TOO_LONG;
}
num_reqs = (tag->size + (data_per_packet-1)) / data_per_packet;
pdebug(debug,"We need %d requests.", num_reqs);
byte_offset = 0;
for(i=0; i<num_reqs && rc == PLCTAG_STATUS_OK; i++) {
/* allocate a new slot */
rc = allocate_write_request_slot(tag);
if(rc == PLCTAG_STATUS_OK) {
/* how much data are we going to write in this packet? */
if((tag->size - byte_offset) > data_per_packet) {
tag->write_req_sizes[i] = data_per_packet;
} else {
tag->write_req_sizes[i] = (tag->size - byte_offset);
}
pdebug(debug,"Request %d is of size %d.",i,tag->write_req_sizes[i]);
/* update the byte offset for the next packet */
byte_offset += tag->write_req_sizes[i];
}
}
pdebug(debug,"Done.");
return rc;
}
static int SetNsf(nsf_t * nsf)
{
int i,j;
unsigned int newsize;
unsigned int total_ms;
pdebug("nsf : Set nsf %p\n", nsf);
if (app.nsf) {
pdebug("nsf : nsf already setted\n");
return -1;
}
if (!nsf) {
goto error;
}
/* Setup PCM buffer. */
newsize = 44100 / nsf->playback_rate;
pdebug("nsf : buffer-size [%u]\n", newsize);
if (newsize > app.bufmax) {
pdebug("nsf : buffer need realloc\n");
app.buffer =
realloc(app.buffer, newsize * 2 + 32);
if (!app.buffer) {
app.bufmax = 0;
} else {
app.bufmax = newsize;
}
}
if (newsize > app.bufmax) {
goto error;
}
app.bufcnt = 0;
app.buflen = newsize;
/* Setup track-list. */
pdebug("nsf : set track times\n");
for (i=1, j=0, total_ms = 0; i <= nsf->num_songs; ++i) {
unsigned int ms =
nsf->song_frames[i] * 1000u / (unsigned int)nsf->playback_rate;
if (ms < 5000u) {
continue;
}
++j;
app.tracks[j].map = i;
app.tracks[j].ms = ms;
total_ms += ms;
pdebug("nsf : track #%02d, mapped on #%02d, ms=%u\n",j,i,ms);
}
app.tracks[0].map = 0;
app.tracks[0].ms = total_ms;
if (!j) {
goto error;
}
app.n_tracks = j;
app.cur_track = 0;
app.nsf = nsf;
return 0;
error:
memset(&app, 0, sizeof(app));
return -1;
}
/*
* allocate_request_slot
*
* Increase the number of available request slots.
*/
int allocate_request_slot(ab_tag_p tag)
{
int *old_sizes;
ab_request_p *old_reqs;
int i;
int old_max = tag->max_requests;
int debug = tag->debug;
pdebug(debug, "Starting.");
/* bump up the number of allowed requests */
tag->max_requests += DEFAULT_MAX_REQUESTS;
pdebug(debug, "setting max_requests = %d", tag->max_requests);
/* (re)allocate the read size array */
old_sizes = tag->read_req_sizes;
tag->read_req_sizes = (int*)mem_alloc(tag->max_requests * sizeof(int));
if(!tag->read_req_sizes) {
mem_free(old_sizes);
tag->status = PLCTAG_ERR_NO_MEM;
return tag->status;
}
/* copy the size data */
if(old_sizes) {
for(i=0; i < old_max; i++) {
tag->read_req_sizes[i] = old_sizes[i];
}
mem_free(old_sizes);
}
/* (re)allocate the write size array */
old_sizes = tag->write_req_sizes;
tag->write_req_sizes = (int*)mem_alloc(tag->max_requests * sizeof(int));
if(!tag->write_req_sizes) {
mem_free(old_sizes);
tag->status = PLCTAG_ERR_NO_MEM;
return tag->status;
}
/* copy the size data */
if(old_sizes) {
for(i=0; i < old_max; i++) {
tag->write_req_sizes[i] = old_sizes[i];
}
mem_free(old_sizes);
}
/* do the same for the request array */
old_reqs = tag->reqs;
tag->reqs = (ab_request_p*)mem_alloc(tag->max_requests * sizeof(ab_request_p));
if(!tag->reqs) {
tag->status = PLCTAG_ERR_NO_MEM;
return tag->status;
}
/* copy the request data, there shouldn't be anything here I think... */
if(old_reqs) {
for(i=0; i < old_max; i++) {
tag->reqs[i] = old_reqs[i];
}
mem_free(old_reqs);
}
return PLCTAG_STATUS_OK;
}
static void * play_loop(void *arg)
{
nsf_t * nsf = app.nsf;
pdebug("nsf : play-thread\n");
if (!app.nsf) {
going = FALSE;
}
pdebug("nsf : going [%s]\n", going ? "Yes" : "No");
app.cur_track = 0;
nsf->cur_frame = nsf->cur_frame_end = 0;
while (going) {
if (nsf->cur_frame >= nsf->cur_frame_end) {
int next_track = app.cur_track+1;
pdebug("nsf : frame elapsed [%u > %u] -> track #%02d\n",
nsf->cur_frame,nsf->cur_frame_end, next_track);
if (next_track > app.n_tracks) {
pdebug("nsf : reach last song\n");
going = FALSE;
break;
}
SetChangeTrack(next_track, -1);
}
if (ApplyChangeTrack() < 0) {
going = FALSE;
break;
}
if (!app.bufcnt) {
/* Run a frame. */
nsf_frame(nsf);
apu_process(app.buffer, app.buflen);
app.bufptr = app.buffer;
app.bufcnt = app.buflen;
}
if (going) {
int tosend = app.bufcnt << 1;
while (nosefart_ip.output->buffer_free() < tosend && going) {
xmms_usleep(30000);
}
nosefart_ip.add_vis_pcm(nosefart_ip.output->written_time(),
FMT_S16_LE, 1, tosend, app.bufptr);
nosefart_ip.output->write_audio(app.bufptr, tosend);
tosend >>= 1;
app.bufcnt -= tosend;
app.bufptr += tosend;
}
}
pdebug("nsf : decoder loop end\n");
SetChangeTrack(0, -1);
ApplyChangeTrack();
if (app.nsf) {
nsf_free(&app.nsf);
app.nsf = nsf = 0;
}
/* Make sure the output plugin stops prebuffering */
if (nosefart_ip.output) {
pdebug("nsf : free audio buffer.\n");
nosefart_ip.output->buffer_free();
}
going = FALSE;
pdebug("nsf : decode finish\n");
play_thread = 0;
pthread_exit(NULL);
}
int build_write_request(ab_tag_p tag, int slot, int byte_offset)
{
int rc = PLCTAG_STATUS_OK;
int debug = tag->debug;
eip_cip_uc_req *cip;
uint8_t *data;
uint8_t *embed_start, *embed_end;
ab_request_p req = NULL;
pdebug(debug,"Starting.");
/* get a request buffer */
rc = request_create(&req);
if(rc != PLCTAG_STATUS_OK) {
pdebug(debug,"Unable to get new request. rc=%d",rc);
tag->status = rc;
return rc;
}
/* set debug flag on the request too */
req->debug = tag->debug;
cip = (eip_cip_uc_req*)(req->data);
/* point to the end of the struct */
data = (req->data) + sizeof(eip_cip_uc_req);
/*
* set up the embedded CIP read packet
* The format is:
*
* uint8_t cmd
* LLA formatted name
* data type to write
* uint16_t # of elements to write
* data to write
*/
embed_start = data;
/*
* set up the CIP Read request type.
* Different if more than one request.
*
* This handles a bug where attempting fragmented requests
* does not appear to work with a single boolean.
*/
*data = (tag->num_write_requests > 1) ? AB_EIP_CMD_CIP_WRITE_FRAG : AB_EIP_CMD_CIP_WRITE;
data++;
/* copy the tag name into the request */
mem_copy(data,tag->encoded_name,tag->encoded_name_size);
data += tag->encoded_name_size;
/* copy encoded type info */
if(tag->encoded_type_info_size) {
mem_copy(data,tag->encoded_type_info,tag->encoded_type_info_size);
data += tag->encoded_type_info_size;
} else {
tag->status = PLCTAG_ERR_UNSUPPORTED;
return tag->status;
}
/* copy the item count, little endian */
*((uint16_t*)data) = h2le16(tag->elem_count);
data += 2;
if(tag->num_write_requests > 1) {
/* put in the byte offset */
*((uint32_t*)data) = h2le32(byte_offset);
data += 4;
}
/* now copy the data to write */
mem_copy(data, tag->data + byte_offset, tag->write_req_sizes[slot]);
data += tag->write_req_sizes[slot];
/* need to pad data to multiple of 16-bits */
if(tag->write_req_sizes[slot] & 0x01) {
*data = 0;
data++;
}
/* mark the end of the embedded packet */
embed_end = data;
/*
* after the embedded packet, we need to tell the message router
* how to get to the target device.
*/
/* Now copy in the routing information for the embedded message */
*data = (tag->conn_path_size)/2; /* in 16-bit words */
data++;
*data = 0;
data++;
mem_copy(data,tag->conn_path, tag->conn_path_size);
data += tag->conn_path_size;
/* now fill in the rest of the structure. */
/* encap fields */
cip->encap_command = h2le16(AB_EIP_READ_RR_DATA); /* ALWAYS 0x006F Unconnected Send*/
/* router timeout */
cip->router_timeout = h2le16(1); /* one second timeout, enough? */
/* Common Packet Format fields for unconnected send. */
cip->cpf_item_count = h2le16(2); /* ALWAYS 2 */
cip->cpf_nai_item_type = h2le16(AB_EIP_ITEM_NAI); /* ALWAYS 0 */
cip->cpf_nai_item_length = h2le16(0); /* ALWAYS 0 */
cip->cpf_udi_item_type = h2le16(AB_EIP_ITEM_UDI); /* ALWAYS 0x00B2 - Unconnected Data Item */
cip->cpf_udi_item_length = h2le16(data - (uint8_t*)(&(cip->cm_service_code))); /* REQ: fill in with length of remaining data. */
/* CM Service Request - Connection Manager */
cip->cm_service_code = AB_EIP_CMD_UNCONNECTED_SEND; /* 0x52 Unconnected Send */
cip->cm_req_path_size = 2; /* 2, size in 16-bit words of path, next field */
cip->cm_req_path[0] = 0x20; /* class */
cip->cm_req_path[1] = 0x06; /* Connection Manager */
cip->cm_req_path[2] = 0x24; /* instance */
cip->cm_req_path[3] = 0x01; /* instance 1 */
/* Unconnected send needs timeout information */
cip->secs_per_tick = AB_EIP_SECS_PER_TICK; /* seconds per tick */
cip->timeout_ticks = AB_EIP_TIMEOUT_TICKS; /* timeout = srd_secs_per_tick * src_timeout_ticks */
/* size of embedded packet */
cip->uc_cmd_length = h2le16(embed_end - embed_start);
/* set the size of the request */
req->request_size = data - (req->data);
/* mark it as ready to send */
req->send_request = 1;
/* add the request to the session's list. */
rc = request_add(tag->session, req);
if(rc != PLCTAG_STATUS_OK) {
pdebug(debug,"Unable to lock add request to session! rc=%d",rc);
plc_tag_abort((plc_tag)tag);
request_destroy(&req);
tag->status = rc;
return rc;
}
/* save the request for later */
tag->reqs[slot] = req;
pdebug(debug,"Done");
return PLCTAG_STATUS_OK;
}
int WindscreenLocator::locateLeftRight()
{
CvRect roiRect = cvRect(0, imgGradH->height / 3, imgGradH->width, imgGradH->height * 0.9 - imgGradH->height/3);
thresholdGrad = max(averageGrad, 1.0 ) * 2.5;
cvSetImageROI(imgGradV, roiRect);
histoStat(imgGradV, vertical2X, NULL, thresholdGrad, 0);
cvResetImageROI(imgGradV);
cvSetImageROI(imgGradH, roiRect);
histoStat(imgGradH, horizon2X, NULL, thresholdGrad, 0);
cvResetImageROI(imgGradH);
int margin = 0.04 * imgGrad->width;
memset(vertical2X, 0, sizeof(double) * margin);
memset(vertical2X + imgGrad->width - margin, 0, sizeof(double) * margin);
histoSmooth(vertical2X, imgGrad->width, 5);
histoSmooth(vertical2X, imgGrad->width, 10);
histoSmooth(vertical2X, imgGrad->width, 20);
histoSmooth(horizon2X, imgGrad->width, 5);
histoSmooth(horizon2X, imgGrad->width, 10);
histoSmooth(horizon2X, imgGrad->width, 40);
// 计算车辆左右边界
int topList[10];
int topNr = 3;
histoCrest(vertical2X, imgGrad->width, topList, topNr);
if(!plate->isValid()){
if(topNr >= 2){
int a = min(topList[0], topList[1]);
int b = max(topList[0], topList[1]);
pdebug("horizon=%f\n", horizon2X[(b+a) / 2]);
pdebug("bike Width = %d, lpWIdth=%f\n", b - a, plate->defaultWidth *2);
double f = isDay(imgGray) ? 0.09 : 0.07;
if(horizon2X[(b+a) / 2] > imgRGB->height * (0.9 - 1.0 / 3) * f){
}else if((double)(b - a) < plate->defaultWidth * 2){
if(b - a < plate->defaultWidth * 1.5){
int ext = -(b - a - plate->defaultWidth * 1.5) / 2;
a = max((a - ext), 0);
b = min((b + ext), (imgGrad->width - 1));
}
if(existCrest(horizon2X, imgGrad->width, a, b, 0.9) != 0){
return -1;
}
}else if((double)(b - a) < plate->defaultWidth * 3.2 && b < imgRGB->width * 0.7){
if(existCrest(horizon2X, imgGrad->width, a, b, 0.8) != 0){
CvRect rect;
double shrunkRatio = imgRGB->width / double(imgOrigin->width);
rect.x = a;
rect.width = b - a;
rect.y = imgRGB->height * 0.25;
rect.height = imgRGB->height * (0.3 - 0.25);
rect = rectScale(rect, 1 / shrunkRatio, cvSize(imgOrigin->width, imgOrigin->height));
vector<FaceInfo> faceVec;
faceDetector->detectTriWheelMotor(imgOrigin, rect, plate->defaultWidth, faceVec);
if(faceVec.size() < 1){
return -1;
}
bool isTriWheel = true;
for(size_t i = 0; i < faceVec.size(); i++){
int mid = faceVec[i].getMiddlePoint().x;
if(mid < rect.x + 0.4 * rect.width || mid > rect.x + 0.6 * rect.width){
isTriWheel = false;
pdebug("face pos=%d %d\n", faceVec[i].getMiddlePoint().x, faceVec[i].getMiddlePoint().y);
break;
}
}
if(isTriWheel)
return -1;
}
}
}
}
int intervalA;
int intervalB;
histoSegment(horizon2X, imgGrad->width, histoSegmentRatio(averageGrad), intervalA, intervalB);
judgeVerticalBorder(imgGrad->width, topList, topNr, intervalA, intervalB, winLeft, winRight);
return 0;
}
static int check_write_status(ab_tag_p tag)
{
eip_cip_uc_resp *cip_resp;
int rc = PLCTAG_STATUS_OK;
int i;
ab_request_p req;
int debug = tag->debug;
/* is there an outstanding request? */
if(!tag->reqs) {
tag->write_in_progress = 0;
tag->status = PLCTAG_ERR_NULL_PTR;
return PLCTAG_ERR_NULL_PTR;
}
for(i = 0; i < tag->num_write_requests; i++) {
if(tag->reqs[i] && !tag->reqs[i]->resp_received) {
tag->status = PLCTAG_STATUS_PENDING;
return PLCTAG_STATUS_PENDING;
}
}
/*
* process each request. If there is more than one request, then
* we need to make sure that we copy the data into the right part
* of the tag's data buffer.
*/
for(i = 0; i < tag->num_write_requests; i++) {
int reply_service;
req = tag->reqs[i];
if(!req) {
rc = PLCTAG_ERR_NULL_PTR;
break;
}
/* point to the data */
cip_resp = (eip_cip_uc_resp*)(req->data);
if(le2h16(cip_resp->encap_command) != AB_EIP_READ_RR_DATA) {
pdebug(debug,"Unexpected EIP packet type received: %d!",cip_resp->encap_command);
rc = PLCTAG_ERR_BAD_DATA;
break;
}
if(le2h16(cip_resp->encap_status) != AB_EIP_OK) {
pdebug(debug,"EIP command failed, response code: %d",cip_resp->encap_status);
rc = PLCTAG_ERR_REMOTE_ERR;
break;
}
/* if we have fragmented the request, we need to look for a different return code */
reply_service = ((tag->num_write_requests > 1) ? (AB_EIP_CMD_CIP_WRITE_FRAG | AB_EIP_CMD_CIP_OK) : (AB_EIP_CMD_CIP_WRITE | AB_EIP_CMD_CIP_OK));
if(cip_resp->reply_service != reply_service) {
pdebug(debug,"CIP response reply service unexpected: %d",cip_resp->reply_service);
rc = PLCTAG_ERR_BAD_DATA;
break;
}
if(cip_resp->status != AB_CIP_STATUS_OK && cip_resp->status != AB_CIP_STATUS_FRAG) {
pdebug(debug,"CIP read failed with status: %d",cip_resp->status);
pdebug(debug,cip_decode_status(cip_resp->status));
rc = PLCTAG_ERR_REMOTE_ERR;
break;
}
}
/*
* Now remove the requests from the session's request list.
*
* FIXME - this has been removed in favor of making the
* IO thread do the clean up.
*/
//for(i = 0; i < tag->num_write_requests; i++) {
// int tmp_rc;
// req = tag->reqs[i];
// tmp_rc = request_remove(tag->session, req);
// if(tmp_rc != PLCTAG_STATUS_OK) {
// pdebug(debug,"Unable to remove the request from the list! rc=%d",rc);
// /* since we could not remove it, maybe the thread can. */
// req->abort_request = 1;
// rc = tmp_rc;
// } else {
// /* free up the request resources */
// request_destroy(&req);
// }
// /* mark it as freed */
// tag->reqs[i] = NULL;
//}
/* this triggers the clean up */
ab_tag_abort(tag);
tag->write_in_progress = 0;
tag->status = rc;
pdebug(debug,"Done.");
return rc;
}
void WindscreenLocator::locateBottomTop2()
{
int margin = (winRight - winLeft) * 0.22;
int height = winBottom - winTop;
cvSetImageROI(imgRGB, cvRect(winLeft + margin, winTop, winRight - winLeft - margin * 2, height));
double* vertical2Y = (double*)malloc(sizeof(double) * height);
if(!vertical2Y){
fprintf(stderr, "malloc failed\n");
fflush(stderr);
exit(-1);
}
memset(vertical2Y, 0, sizeof(double) * height);
IplImage* ver;
IplImage* hor;
IplImage* sla;
IplImage* res;
diagonal_origin(imgRGB, hor, ver, sla, res);
double thres = statThreshold(ver);
histoStat(ver, NULL, vertical2Y, thres * 0.65, 0);
cvResetImageROI(imgRGB);
histoSmooth(vertical2Y, height, 5);
histoSmooth(vertical2Y, height, 10);
histoSmooth(vertical2Y, height, 20);
int intervalA = 0;
int intervalB = height - 1;
histoSegment(vertical2Y, height, 0.5, intervalA, intervalB, 0.7);
vertical2Y[intervalA] = 100;
vertical2Y[intervalB] = 80;
cvReleaseImage(&ver);
cvReleaseImage(&hor);
cvReleaseImage(&sla);
cvReleaseImage(&res);
free(vertical2Y);
if((plate->isValid() && (plate->getMiddlePoint().x > winLeft
&& plate->getMiddlePoint().x < winRight))
|| reviser != NULL){
int plateWidth = 35;
if(plate->isValid())
plateWidth= plate->getPlateWidth();
if(reviser && reviser->getPlateWidth() > 0)
plateWidth = reviser->getPlateWidth();
if(winRight - winLeft < 4.5 * plateWidth){
if(intervalA != 0 && intervalB - intervalA > plateWidth * 0.65){
pdebug("[%d %d]\n", intervalA, intervalB);
winBottom = intervalB + winTop;
winTop = intervalA + winTop;
// int newHeight = intervalB - intervalA;
int w = winRight - winLeft;
winBottom += w * 0.05;
winTop -= w * 0.14;
}
}
}
winBottom = std::max(0, winBottom);
winBottom = std::min(imgRGB->height - 1, winBottom);
winTop = std::min(winTop, winBottom - 1);
winTop = std::max(0, winTop);
}
int eip_pccc_tag_write_start(ab_tag_p tag)
{
int rc = PLCTAG_STATUS_OK;
pccc_req *pccc;
uint8_t *data;
uint8_t element_def[16];
int element_def_size;
uint8_t array_def[16];
int array_def_size;
int pccc_data_type;
uint16_t conn_seq_id = (uint16_t)(session_get_new_seq_id(tag->session));;
ab_request_p req = NULL;
int debug = tag->debug;
uint8_t *embed_start;
int overhead, data_per_packet;
pdebug(debug,"Starting.");
/* how many packets will we need? How much overhead? */
overhead = sizeof(pccc_resp) + 4 + tag->encoded_name_size; /* MAGIC 4 = fudge */
data_per_packet = MAX_PCCC_PACKET_SIZE - overhead;
if(data_per_packet <= 0) {
pdebug(debug,"Unable to send request. Packet overhead, %d bytes, is too large for packet, %d bytes!", overhead, MAX_EIP_PACKET_SIZE);
tag->status = PLCTAG_ERR_TOO_LONG;
return tag->status;
}
if(data_per_packet < tag->size) {
pdebug(debug,"PCCC requests cannot be fragmented. Too much data requested.");
tag->status = PLCTAG_ERR_TOO_LONG;
return tag->status;
}
/* set up the requests */
if(!tag->reqs) {
tag->reqs = (ab_request_p*)mem_alloc(1 * sizeof(ab_request_p));
tag->max_requests = 1;
tag->num_read_requests = 1;
tag->num_write_requests = 1;
if(!tag->reqs) {
pdebug(debug,"Unable to get memory for request array!");
tag->status = PLCTAG_ERR_NO_MEM;
return tag->status;
}
}
/* get a request buffer */
rc = request_create(&req);
if(rc != PLCTAG_STATUS_OK) {
pdebug(debug,"Unable to get new request. rc=%d",rc);
tag->status = rc;
return rc;
}
req->debug = tag->debug;
pccc = (pccc_req*)(req->data);
/* set up the embedded PCCC packet */
embed_start = (uint8_t*)(&pccc->service_code);
/* point to the end of the struct */
data = (req->data) + sizeof(pccc_req);
/* copy laa into the request */
mem_copy(data,tag->encoded_name,tag->encoded_name_size);
data += tag->encoded_name_size;
/* What type and size do we have? */
if(tag->elem_size != 2 && tag->elem_size != 4) {
pdebug(debug,"Unsupported data type size: %d",tag->elem_size);
request_destroy(&req);
tag->status = PLCTAG_ERR_NOT_ALLOWED;
return PLCTAG_ERR_NOT_ALLOWED;
}
if(tag->elem_size == 4)
pccc_data_type = AB_PCCC_DATA_REAL;
else
pccc_data_type = AB_PCCC_DATA_INT;
/* generate the data type/data size fields, first the element part so that
* we can get the size for the array part.
*/
if(!(element_def_size = pccc_encode_dt_byte(element_def,sizeof(element_def),pccc_data_type,tag->elem_size))) {
pdebug(debug,"Unable to encode PCCC request array element data type and size fields!");
request_destroy(&req);
tag->status = PLCTAG_ERR_ENCODE;
return PLCTAG_ERR_ENCODE;
}
if(!(array_def_size = pccc_encode_dt_byte(array_def,sizeof(array_def),AB_PCCC_DATA_ARRAY,element_def_size + tag->size))) {
pdebug(debug,"Unable to encode PCCC request data type and size fields!");
request_destroy(&req);
tag->status = PLCTAG_ERR_ENCODE;
return PLCTAG_ERR_ENCODE;
}
/* copy the array data first. */
mem_copy(data,array_def,array_def_size);
data += array_def_size;
/* copy the element data */
mem_copy(data,element_def,element_def_size);
data += element_def_size;
/* now copy the data to write */
mem_copy(data,tag->data,tag->size);
data += tag->size;
/* now fill in the rest of the structure. */
/* encap fields */
pccc->encap_command = h2le16(AB_EIP_READ_RR_DATA); /* ALWAYS 0x0070 Unconnected Send*/
/* router timeout */
pccc->router_timeout = h2le16(1); /* one second timeout, enough? */
/* Common Packet Format fields for unconnected send. */
pccc->cpf_item_count = h2le16(2); /* ALWAYS 2 */
pccc->cpf_nai_item_type = h2le16(AB_EIP_ITEM_NAI); /* ALWAYS 0 */
pccc->cpf_nai_item_length = h2le16(0); /* ALWAYS 0 */
pccc->cpf_udi_item_type = h2le16(AB_EIP_ITEM_UDI); /* ALWAYS 0x00B2 - Unconnected Data Item */
pccc->cpf_udi_item_length = h2le16(data - embed_start); /* REQ: fill in with length of remaining data. */
/* Command Routing */
pccc->service_code = AB_EIP_CMD_PCCC_EXECUTE; /* ALWAYS 0x4B, Execute PCCC */
pccc->req_path_size = 2; /* ALWAYS 2, size in words of path, next field */
pccc->req_path[0] = 0x20; /* class */
pccc->req_path[1] = 0x67; /* PCCC Execute */
pccc->req_path[2] = 0x24; /* instance */
pccc->req_path[3] = 0x01; /* instance 1 */
/* PCCC ID */
pccc->request_id_size = 7; /* ALWAYS 7 */
pccc->vendor_id = h2le16(AB_EIP_VENDOR_ID); /* Our CIP Vendor */
pccc->vendor_serial_number = h2le32(AB_EIP_VENDOR_SN); /* our unique serial number */
/* PCCC Command */
pccc->pccc_command = AB_EIP_PCCC_TYPED_CMD;
pccc->pccc_status = 0; /* STS 0 in request */
//pccc->pccc_seq_num = h2le16(tag->connection->conn_seq_num);
pccc->pccc_function = AB_EIP_PCCC_TYPED_WRITE_FUNC;
/* FIXME - what should be the count here? It is bytes, 16-bit
* words or something else?
*
* Seems to be the number of elements??
*/
pccc->pccc_transfer_size = h2le16(tag->elem_count); /* This is not in the docs, but it is in the data. */
/* get ready to add the request to the queue for this session */
req->request_size = data - (req->data);
req->send_request = 1;
req->conn_seq = conn_seq_id;
/* add the request to the session's list. */
rc = request_add(tag->session, req);
if(rc != PLCTAG_STATUS_OK) {
pdebug(debug,"Unable to lock add request to session! rc=%d",rc);
request_destroy(&req);
tag->status = rc;
return rc;
}
/* save the request for later */
tag->reqs[0] = req;
/* the write is now pending */
tag->write_in_progress = 1;
tag->status = PLCTAG_STATUS_PENDING;
return PLCTAG_STATUS_PENDING;
}
void WindscreenLocator::locateBottomTop()
{
cvSetImageROI(imgGradH, cvRect(winLeft, 0, winRight - winLeft, imgGradH->height));
histoStat(imgGradH, NULL, horizon2Y, thresholdGrad, 0);
cvResetImageROI(imgGradH);
histoSmooth(horizon2Y, imgGrad->height, 5);
histoSmooth(horizon2Y, imgGrad->height, 10);
histoSmooth(horizon2Y, imgGrad->height, 10);
int yCrest[20];
int yCrestNr = 20;
histoCrest(horizon2Y, imgGrad->height, yCrest, yCrestNr);
int crest = 0;
bool usePrediction = false;
winTop = 0;
winBottom = imgRGB->height;
// if(plate->isValid() && reviser != NULL){
if(plate->isValid()
&& (plate->getMiddlePoint().x > winLeft
&& plate->getMiddlePoint().x < winRight)){
int plateWidth = plate->getPlateWidth();
if(reviser && reviser->getPlateWidth() > 0)
plateWidth = reviser->getPlateWidth();
CvPoint mid = plate->getMiddlePoint();
if(winRight - winLeft > 4.5 * plateWidth)
winBottom = mid.y - 80 / 35.0 * plateWidth;
else
winBottom = mid.y - 60.0/35.0 * plateWidth;
// crest = findLikelytCrest(horizon2Y, imgGrad->height, winBottom, winBottom - plateWidth * 0.3, winBottom + plateWidth * 0.3);
pdebug("winBottom=%d, crest=%d\n", winBottom, crest);
if(crest > 0 && crest < imgRGB->height)
winBottom = crest;
if(reviser)
reviser->statLowerBorder(plate, winLeft, winRight, winBottom);
}else if(reviser != NULL){
int plateWidth = reviser->getPlateWidth();
usePrediction = true;
pdebug("License not Found, Use Prediction.\n");
winBottom = reviser->predictLowerBorder(plate, winLeft, winRight);
// crest = findLikelytCrest(horizon2Y, imgGrad->height, winBottom, winBottom - plateWidth * 0.3, winBottom + plateWidth * 0.3);
crest = reviser->predictLowerBorder(plate, winLeft, winRight); //
if(crest == 0){
crest = imgRGB->height - 1;
winTop = imgRGB->height * 0.25;
winBottom = imgRGB->height * 0.9;
winBottom = std::max(0, winBottom);
winBottom = std::min(imgRGB->height - 1, winBottom);
winTop = std::min(winTop, winBottom - 1);
winTop = std::max(0, winTop);
return;
}
if(crest <= winBottom)
crest += 0.5 * plateWidth; // 预测的放宽范围
if(crest > 0 && crest < imgRGB->height)
winBottom = crest;
}
if(!usePrediction){
if(plate->isValid())
winTop = winBottom - (winRight - winLeft) * 0.65;
else
winTop = 0;
}else{
// winTop = winBottom - (winRight - winLeft) * 0.60; // 预测的放宽范围
winTop = winBottom - (winRight - winLeft) * 0.75; // 预测的放宽范围
}
winBottom = std::max(0, winBottom);
winBottom = std::min(imgRGB->height - 1, winBottom);
winTop = std::min(winTop, winBottom - 1);
winTop = std::max(0, winTop);
}
int eip_pccc_tag_read_start(ab_tag_p tag)
{
int rc = PLCTAG_STATUS_OK;
ab_request_p req;
uint16_t conn_seq_id = (uint16_t)(session_get_new_seq_id(tag->session));;
int overhead;
int data_per_packet;
pccc_req *pccc;
uint8_t *data;
uint8_t *embed_start;
int debug = tag->debug;
pdebug(debug,"Starting");
/* how many packets will we need? How much overhead? */
overhead = sizeof(pccc_resp) + 4 + tag->encoded_name_size; /* MAGIC 4 = fudge */
data_per_packet = MAX_PCCC_PACKET_SIZE - overhead;
if(data_per_packet <= 0) {
pdebug(debug,"Unable to send request. Packet overhead, %d bytes, is too large for packet, %d bytes!", overhead, MAX_EIP_PACKET_SIZE);
tag->status = PLCTAG_ERR_TOO_LONG;
return tag->status;
}
if(data_per_packet < tag->size) {
pdebug(debug,"PCCC requests cannot be fragmented. Too much data requested.");
tag->status = PLCTAG_ERR_TOO_LONG;
return tag->status;
}
if(!tag->reqs) {
tag->reqs = (ab_request_p*)mem_alloc(1 * sizeof(ab_request_p));
tag->max_requests = 1;
tag->num_read_requests = 1;
tag->num_write_requests = 1;
if(!tag->reqs) {
pdebug(debug,"Unable to get memory for request array!");
tag->status = PLCTAG_ERR_NO_MEM;
return tag->status;
}
}
/* get a request buffer */
rc = request_create(&req);
if(rc != PLCTAG_STATUS_OK) {
pdebug(debug,"Unable to get new request. rc=%d",rc);
tag->status = rc;
return rc;
}
req->debug = tag->debug;
/* point the struct pointers to the buffer*/
pccc = (pccc_req*)(req->data);
/* set up the embedded PCCC packet */
embed_start = (uint8_t*)(&pccc->service_code);
/* Command Routing */
pccc->service_code = AB_EIP_CMD_PCCC_EXECUTE; /* ALWAYS 0x4B, Execute PCCC */
pccc->req_path_size = 2; /* ALWAYS 2, size in words of path, next field */
pccc->req_path[0] = 0x20; /* class */
pccc->req_path[1] = 0x67; /* PCCC Execute */
pccc->req_path[2] = 0x24; /* instance */
pccc->req_path[3] = 0x01; /* instance 1 */
/* PCCC ID */
pccc->request_id_size = 7; /* ALWAYS 7 */
pccc->vendor_id = h2le16(AB_EIP_VENDOR_ID); /* Our CIP Vendor */
pccc->vendor_serial_number = h2le32(AB_EIP_VENDOR_SN); /* our unique serial number */
/* fill in the PCCC command */
pccc->pccc_command = AB_EIP_PCCC_TYPED_CMD;
pccc->pccc_status = 0; /* STS 0 in request */
pccc->pccc_seq_num = h2le16(conn_seq_id); /* FIXME - get sequence ID from session? */
pccc->pccc_function = AB_EIP_PCCC_TYPED_READ_FUNC;
pccc->pccc_transfer_size = h2le16(tag->elem_count); /* This is not in the docs, but it is in the data. */
/* point to the end of the struct */
data = ((uint8_t *)pccc) + sizeof(pccc_req);
/* copy LAA tag name into the request */
mem_copy(data,tag->encoded_name,tag->encoded_name_size);
data += tag->encoded_name_size;
/* we need the count twice? */
*((uint16_t*)data) = h2le16(tag->elem_count); /* FIXME - bytes or INTs? */
data += sizeof(uint16_t);
/*
* after the embedded packet, we need to tell the message router
* how to get to the target device.
*/
/* encap fields */
pccc->encap_command = h2le16(AB_EIP_READ_RR_DATA); /* ALWAYS 0x0070 Unconnected Send*/
/* router timeout */
pccc->router_timeout = h2le16(1); /* one second timeout, enough? */
/* Common Packet Format fields for unconnected send. */
pccc->cpf_item_count = h2le16(2); /* ALWAYS 2 */
pccc->cpf_nai_item_type = h2le16(AB_EIP_ITEM_NAI); /* ALWAYS 0 */
pccc->cpf_nai_item_length = h2le16(0); /* ALWAYS 0 */
pccc->cpf_udi_item_type = h2le16(AB_EIP_ITEM_UDI); /* ALWAYS 0x00B2 - Unconnected Data Item */
pccc->cpf_udi_item_length = h2le16(data - embed_start); /* REQ: fill in with length of remaining data. */
/* set the size of the request */
req->request_size = data - (req->data);
/* mark it as ready to send */
req->send_request = 1;
/* add the request to the session's list. */
rc = request_add(tag->session, req);
if(rc != PLCTAG_STATUS_OK) {
pdebug(debug,"Unable to lock add request to session! rc=%d",rc);
request_destroy(&req);
tag->status = rc;
return rc;
}
/* save the request for later */
tag->reqs[0] = req;
req = NULL;
tag->read_in_progress = 1;
tag->status = PLCTAG_STATUS_PENDING;
return PLCTAG_STATUS_PENDING;
}
// Emit rectangle, width and height are inclusive
int surface_height::height_rectangle_emit(MID_POINT_CONFIG *p_config,
int x, int y, int width, int height,
int iteration,
SQUARE_TYPE type,
HEIGHT_WRITE_FUNC func)
{
int iteration_target = p_config->iteration_target;
int pixels = 0;
int max_distance = p_config->pixel_distance;
if(max_distance < 0) {
max_distance = 0;
}
// 3x3 and bigger
if(width >= max_distance && height >= max_distance) {
tpos mdx = width/2,
mdy = height/2;
int mdx_correction = (width&0x1) ? mdx+1 : mdx;
int mdy_correction = (height&0x1) ? mdy+1 : mdy;
tpos mx = x + mdx,
my = y + mdy;
tpos dist = MIN(mdx,mdy);
/*
Generate corner points
*-------*
| |
| |
| |
*-------*
*/
// Generate only when those points are undefined
if(!p_config->height_overwrite &&
iteration == 0 && iteration_target == iteration)
{
float height_current = p_config->border_start_height;
float height_base = p_config->border_start_height;
if(p_config->border_start_height == FLOAT_UNDEFINED) {
height_current = height_generate(p_config, x, y, height_base,
iteration);
}
height_set(x,y, height_current);
pixels++;
if(p_config->border_start_height == FLOAT_UNDEFINED) {
height_current = height_generate(p_config, x+width, y, height_base,
iteration);
}
height_set(x+width,y, height_current);
pixels++;
if(p_config->border_start_height == FLOAT_UNDEFINED) {
height_current = height_generate(p_config, x, y+height, height_base,
iteration);
}
height_set(x,y+height, height_current);
pixels++;
if(p_config->border_start_height == FLOAT_UNDEFINED) {
height_current = height_generate(p_config, x+width, y+height,
height_base, iteration);
}
height_set(x+width,y+height, height_current);
pixels++;
}
/*
Generate center of the rectangle
*-------*
| | |
|---o---|
| | |
*-------*
*/
if(iteration == iteration_target) {
// Always overwrite this one -> it clears artifacts generated by
// cross-interpolator
if(1) {
// Interpolate midle of the given rectangle
float h0 = height_get(x,y);
float h1 = height_get(x+width,y+height);
float h2 = height_get(x+width,y);
float h3 = height_get(x,y+height);
pdebug(DEBUG_HEIGHT,"C1: [%d, %d], [%d, %d], [%d, %d], [%d, %d]",
x,y, x+width,y+height, x+width,y, x,y+height);
SQUARE_INTERPOLATION_TYPE itype = p_config->interpolation_get(MIN(width,height));
bool generate = p_config->generation_get(MIN(width,height));
(this->*func)(p_config, mx, my, h0, h1, h2, h3, type, itype, iteration, generate);
pixels++;
}
return(pixels);
}
iteration++;
/*
Generate
*---2---*
| | |
1---*---3
| | |
*---4---*
Types:
*---1---*
| | |
2---*---2
| | |
*---1---*
*/
if(iteration == iteration_target) {
SQUARE_INTERPOLATION_TYPE itype = p_config->interpolation_get(MIN(width,height));
bool generate = p_config->generation_get(MIN(width,height));
// 1
pixels += height_interate_cross(p_config, x, my, dist, iteration,
SQUARE_TYPE_2, itype, generate, func);
// 2
pixels += height_interate_cross(p_config, mx, y, dist, iteration,
SQUARE_TYPE_1, itype, generate, func);
// 3
pixels += height_interate_cross(p_config, x+width, my, dist, iteration,
SQUARE_TYPE_2, itype, generate, func);
// 4
pixels += height_interate_cross(p_config, mx, y+height, dist, iteration,
SQUARE_TYPE_1, itype, generate, func);
return(pixels);
}
iteration++;
/*
Target areas:
*-------*
| 1 | 2 |
|---*---|
| 3 | 4 |
*-------*
Types:
*-------*
| 1 | 2 |
|---*---|
| 2 | 1 |
*-------*
*/
if(iteration <= iteration_target && mdx > 0 && mdy > 0) {
// 1
pixels += height_rectangle_emit(p_config, x, y, mdx, mdy,
iteration, SQUARE_TYPE_1, func);
// 2
pixels += height_rectangle_emit(p_config, mx, y, mdx_correction, mdy,
iteration, SQUARE_TYPE_2, func);
// 3
pixels += height_rectangle_emit(p_config, x, my, mdx, mdy_correction,
iteration, SQUARE_TYPE_2, func);
// 4
pixels += height_rectangle_emit(p_config, mx, my, mdx_correction, mdy_correction,
iteration, SQUARE_TYPE_1, func);
}
}
return(pixels);
}
