void cxl_afu_free(struct cxl_afu_h *afu) {
// Check for valid AFU
if (!afu) return;
// Wait for job_done
while (afu->running) {
// FIXME: Timeout
short_delay();
}
// Reset AFU
status.cmd.code = PSL_JOB_RESET;
status.cmd.addr = 0;
status.cmd.request = AFU_REQUEST;
// Wait for job_done
while (status.cmd.request != AFU_IDLE) {
// FIXME: Timeout
short_delay();
}
// Stop PSL thread
status.psl_state = PSL_DONE;
pthread_join(afu->thread, NULL);
// Shut down socket connection
psl_close_afu_event (status.event);
// Free memory
free (afu);
}
/****************** **********************
函数名：HD7279字节接收函数
功 能：从HD7279接收一个字节
输 入：无
返 回：一个字节
备 注：HD7279硬件函数
******************************************/
unsigned char receive_byte(void)
{
unsigned char i,in_byte;
WR7279|=clk7279; //clk7279=1;
DR7279&=~dat7279; //改dat7279为输入
long_delay();
for(i=0;i<8;i++)
{
WR7279|=clk7279; //clk7279=1;
short_delay();
in_byte=in_byte<<1;
if(RD7279&dat7279)
{
in_byte=in_byte|0x01;
}
WR7279&=~clk7279; //clk7279=0;
short_delay();
}
DR7279|=dat7279; //dat7279还原为输出口
return(in_byte);
}
int cxl_afu_attach(struct cxl_afu_h *afu, __u64 wed) {
if (afu->attached) {
errno = EINVAL;
return -1;
}
// FIXME: Add timeout
while (status.cmd.request != AFU_IDLE) short_delay();
// Start AFU
status.cmd.code = PSL_JOB_START;
status.cmd.addr = wed;
status.cmd.request = AFU_REQUEST;
// FIXME: Add timeout
while (status.cmd.request == AFU_REQUEST) short_delay();
// Wait for job_running
while (!afu->running) {
// FIXME: Timeout
short_delay();
}
afu->attached = 1;
return 0;
}
/****************** **********************
函数名：HD7279字节发送函数
功 能：HD7279发送一个字节
输 入：一个字节
返 回：无
备 注：HD7279硬件函数
******************************************/
void send_byte(unsigned char out_byte)
{
unsigned char i;
WR7279&=~clk7279; //clk7279=0;
WR7279&=~cs7279; //cs7279=0
long_delay();
for(i=0;i<8;i++)
{
if(out_byte&0x80)
{
WR7279|=dat7279; //dat7279=1;
}
else
{
WR7279&=~dat7279; //dat7279=0;
}
WR7279|=clk7279; //clk7279=1;
short_delay();
WR7279&=~clk7279; //clk7279=0;
short_delay();
out_byte=out_byte<<1;
}
WR7279|=dat7279; //dat7279=1;
}
int cxl_mmio_write32(struct cxl_afu_h *afu, uint64_t offset, uint32_t data) {
uint64_t value;
if (offset >= afu->mmio_size)
return -1;
if (offset & 0x3)
return -1;
value = data;
value <<= 32;
value |= data;
#ifdef DEBUG
printf ("Sending MMIO write single word to AFU\n");
#endif /* #ifdef DEBUG */
status.mmio.rnw = 0;
status.mmio.dw = 0;
status.mmio.addr = offset >> 2;
status.mmio.data = value;
status.mmio.request = AFU_REQUEST;
#ifdef DEBUG
printf ("Waiting for MMIO ack from AFU\n");
#endif /* #ifdef DEBUG */
// FIXME: Add timeout
while (status.mmio.request != AFU_IDLE) short_delay();
#ifdef DEBUG
printf ("MMIO write complete\n");
#endif /* #ifdef DEBUG */
return 0;
}
irom static i2c_error_t receive_bit(bool_t *bit)
{
int current, total;
i2c_error_t error;
// at this point scl should be high and sda is unknown,
// but should be high before reading
if(state == i2c_state_idle)
return(i2c_error_invalid_state_idle);
// wait for scl to be released by slave (clock stretching)
if((error = wait_idle()) != i2c_error_ok)
return(error);
// make sure sda is high (open) so slave can pull it
// do it while clock is pulled
clear_scl();
set_sda();
// wait for slave to pull/release sda
delay();
// release clock again
set_scl();
// take care of clock stretching
if((error = wait_idle()) != i2c_error_ok)
return(error);
delay();
// do oversampling of sda, to implement a software
// low-pass filter / spike filter
for(total = 0, current = 0; current < i2c_config_sda_sampling_window; current++)
{
int set;
set = sda_is_set();
total += set ? 4 : 0;
short_delay();
}
if(total < (i2c_config_sda_sampling_window * 1)) // 0-1/4 => 0
*bit = 0;
else if(total < (i2c_config_sda_sampling_window * 3)) // 1/4-3/4 => error
return(i2c_error_receive_error);
else // 3/4-1 => 1
*bit = 1;
return(i2c_error_ok);
}
void LCD_Gotoxy(unsigned char x , unsigned char y)
{
short_delay();
switch(y)
{
case 0 : _lcd_write_command(x + 0x80); break;
case 1 : _lcd_write_command(x + 0xC0); break;
case 2 : _lcd_write_command(x + (0x80 + 20)); break;
case 3 : _lcd_write_command(x + (0xC0 + 20)); break;
}
}
irom static i2c_error_t send_start(void)
{
i2c_error_t error;
int current;
if(state != i2c_state_start_send)
return(i2c_error_invalid_state_not_send_start);
// wait for scl and sda to be released by all masters and slaves
state = i2c_state_bus_wait_1;
if((error = wait_idle()) != i2c_error_ok)
return(error);
// set sda to high
clear_scl();
delay();
if(scl_is_set())
return(i2c_error_bus_lock);
set_sda();
delay();
if(!sda_is_set())
return(i2c_error_sda_stuck);
set_scl();
delay();
state = i2c_state_bus_wait_2;
// demand bus is idle for a minimum window
for(current = i2c_config_scl_sampling_window; current > 0; current--)
{
if(!scl_is_set() || !sda_is_set())
return(i2c_error_bus_lock);
short_delay();
}
// generate start condition by leaving scl high and pulling sda low
clear_sda();
delay();
if(sda_is_set())
return(i2c_error_sda_stuck);
return(i2c_error_ok);
}
/* Output "the_char" 80 times, with delay between
* each to make it easier to interrupt */
void Display_80(const char* the_char)
{
int count=80;
/* Move to new line */
printf(" \r\n");
/* Print "the_char" 80 times with short delay between each */
while(--count >= 0)
{
printf(the_char);
short_delay();
}
}
static int
HarvestAgentByParent(unsigned int *total, int root)
{
int i;
int sts;
int found;
pid_t pid;
AgentInfo *ap;
/* Check for child process termination. Be careful, and ignore any
* non-agent processes found.
*/
do {
found = 0;
pid = root ? waitpid_pmdaroot(&sts) : waitpid_pmcd(&sts);
for (i = 0; i < nAgents; i++) {
ap = &agent[i];
if (!ap->status.connected || ap->ipcType == AGENT_DSO)
continue;
if (root && !ap->status.isRootChild)
continue;
if (!root && !ap->status.isChild)
continue;
found = 1;
if (pid <= (pid_t)0) {
if (total && *total--) {
short_delay(10);
break;
} else {
return -1;
}
}
if (pid == ((ap->ipcType == AGENT_SOCKET)
? ap->ipc.socket.agentPid
: ap->ipc.pipe.agentPid)) {
CleanupAgent(ap, AT_EXIT, sts);
break;
}
}
} while (found);
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "test1_node");
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle node_handle;
try
{
aruco::MarkerDetector MDetector;
vector<aruco::Marker> Markers;
//read the input image
cv::Mat InImage;
InImage=cv::imread("/home/rizi/ros_stuff/aruco_ws/src/aruco_test/images/aruco_marker_1.png");
//Ok, let's detect
MDetector.detect(InImage,Markers);
//for each marker, draw info and its boundaries in the image
for (unsigned int i=0;i<Markers.size();i++) {
std::cout<<Markers[i]<<std::endl;
Markers[i].draw(InImage,cv::Scalar(0,0,255),2);
}
cv::imshow("in",InImage);
cv::waitKey(0);//wait for key to be pressed
} catch (std::exception &ex)
{
cout<<"Exception :"<<ex.what()<<endl;
}
ros::WallDuration short_delay(4.0);
short_delay.sleep();
ros::shutdown();
return 0;
}
int cxl_mmio_read32(struct cxl_afu_h *afu, uint64_t offset, uint32_t *data) {
if (offset >= afu->mmio_size)
return -1;
if (offset & 0x3)
return -1;
#ifdef DEBUG
printf ("Sending MMIO read single word to AFU\n");
#endif /* #ifdef DEBUG */
status.mmio.rnw = 1;
status.mmio.dw = 0;
status.mmio.addr = offset >> 2;
status.mmio.request = AFU_REQUEST;
#ifdef DEBUG
printf ("Waiting for MMIO ack from AFU\n");
#endif /* #ifdef DEBUG */
// FIXME: Add timeout
while (status.mmio.request != AFU_IDLE) short_delay();
*data = (uint32_t) status.mmio.data;
#ifdef DEBUG
printf ("MMIO read complete\n");
#endif /* #ifdef DEBUG */
return 0;
}
void
CleanupAgent(AgentInfo* aPtr, int why, int status)
{
int exit_status = status;
int reason = 0;
if (aPtr->ipcType == AGENT_DSO) {
if (aPtr->ipc.dso.dlHandle != NULL) {
#ifdef HAVE_DLOPEN
dlclose(aPtr->ipc.dso.dlHandle);
#endif
}
pmcd_trace(TR_DEL_AGENT, aPtr->pmDomainId, -1, -1);
}
else {
pmcd_trace(TR_DEL_AGENT, aPtr->pmDomainId, aPtr->inFd, aPtr->outFd);
if (aPtr->inFd != -1) {
if (aPtr->ipcType == AGENT_SOCKET)
__pmCloseSocket(aPtr->inFd);
else {
close(aPtr->inFd);
__pmResetIPC(aPtr->inFd);
}
aPtr->inFd = -1;
}
if (aPtr->outFd != -1) {
if (aPtr->ipcType == AGENT_SOCKET)
__pmCloseSocket(aPtr->outFd);
else {
close(aPtr->outFd);
__pmResetIPC(aPtr->outFd);
}
aPtr->outFd = -1;
}
if (aPtr->ipcType == AGENT_SOCKET &&
aPtr->ipc.socket.addrDomain == AF_UNIX) {
/* remove the Unix domain socket */
unlink(aPtr->ipc.socket.name);
}
}
__pmNotifyErr(LOG_INFO, "CleanupAgent ...\n");
fprintf(stderr, "Cleanup \"%s\" agent (dom %d):", aPtr->pmDomainLabel, aPtr->pmDomainId);
if (why == AT_EXIT) {
/* waitpid has already been done */
fprintf(stderr, " terminated");
reason = (status << 8) | REASON_EXIT;
}
else {
if (why == AT_CONFIG) {
fprintf(stderr, " unconfigured");
} else {
reason = REASON_PROTOCOL;
fprintf(stderr, " protocol failure for fd=%d", status);
exit_status = -1;
}
if (aPtr->status.isChild || aPtr->status.isRootChild) {
pid_t pid = (pid_t)-1;
pid_t done;
int wait_status;
int slept = 0;
if (aPtr->ipcType == AGENT_PIPE)
pid = aPtr->ipc.pipe.agentPid;
else if (aPtr->ipcType == AGENT_SOCKET)
pid = aPtr->ipc.socket.agentPid;
for ( ; ; ) {
done = (aPtr->status.isRootChild) ?
waitpid_pmdaroot(&wait_status):
waitpid_pmcd(&wait_status);
if (done < (pid_t)0)
wait_status = 0;
else if (done == pid) {
exit_status = wait_status;
break;
}
else if (done > 0)
continue;
if (slept)
break;
/* give PMDA a chance to notice the close() and exit */
short_delay(10);
slept = 1;
}
}
}
#ifndef IS_MINGW
if (exit_status != -1) {
if (WIFEXITED(exit_status)) {
fprintf(stderr, ", exit(%d)", WEXITSTATUS(exit_status));
reason = (WEXITSTATUS(exit_status) << 8) | reason;
}
else if (WIFSIGNALED(exit_status)) {
fprintf(stderr, ", signal(%d)", WTERMSIG(exit_status));
#ifdef WCOREDUMP
if (WCOREDUMP(exit_status))
fprintf(stderr, ", dumped core");
#endif
reason = (WTERMSIG(exit_status) << 16) | reason;
}
}
#endif
fputc('\n', stderr);
aPtr->reason = reason;
aPtr->status.connected = 0;
aPtr->status.busy = 0;
aPtr->status.notReady = 0;
aPtr->status.flags = 0;
AgentDied = 1;
if (_pmcd_trace_mask)
pmcd_dump_trace(stderr);
MarkStateChanges(PMCD_DROP_AGENT);
}
struct cxl_afu_h * cxl_afu_open_dev(char *path) {
char *x, *comment, *afu_id, *host, *port_str;
struct cxl_afu_h *afu;
FILE *fp;
char hostdata[MAX_LINE_CHARS];
int port;
uint64_t value;
// Isolate AFU id from full path
x = strrchr (path, '/');
x++;
// Allocate AFU struct
afu = (struct cxl_afu_h *) malloc (sizeof (struct cxl_afu_h));
if (!afu) {
perror ("malloc");
return NULL;
}
// Allocate AFU_EVENT struct
status.event = (struct AFU_EVENT *) malloc (sizeof (struct AFU_EVENT));
if (!status.event ) {
perror ("malloc");
free (afu);
return NULL;
}
psl_event_reset (status.event);
// Connect to AFU server
fp = fopen ("shim_host.dat", "r");
if (!fp) {
perror ("fopen shim_host.dat");
free (status.event);
free (afu);
return NULL;
}
afu_id = x+1;
host = NULL;
port_str = NULL;
while (strcmp (afu_id, x) && fgets (hostdata, MAX_LINE_CHARS, fp)) {
afu_id = hostdata;
comment = strchr(hostdata, '#');
if (comment)
continue;
host = strchr(hostdata, ',');
if (host) {
*host = '\0';
++host;
}
else {
printf ("Invalid format in shim_host.dat. Expected ',' :%s\n",
hostdata);
fclose (fp);
free (status.event);
free (afu);
return NULL;
}
port_str = strchr(host, ':');
if (port_str) {
*port_str = '\0';
++port_str;
}
else {
printf ("Invalid format in shim_host.dat. Expected ':' :%s\n",
hostdata);
fclose (fp);
free (status.event);
free (afu);
return NULL;
}
}
fclose (fp);
// Convert port to int
port = atoi (port_str);
// Connect to AFU server
printf ("Attempting to connect to %s:%d\n", host, port);
if (psl_init_afu_event (status.event, host, port) != PSL_SUCCESS) {
printf ("Unable to connect to %s:%d\n", host, port);
free (status.event);
free (afu);
return NULL;
}
// Start PSL thread
status.psl_state = PSL_INIT;
status.event_occurred = 0;
status.credits = 64;
status.first_br = NULL;
status.last_br = NULL;
afu->id = afu_id;
afu->mmio_size = 0;
afu->attached = 0;
afu->running = 0;
pthread_create(&(afu->thread), NULL, psl, (void *) afu);
psl_aux1_change (status.event, status.credits);
// Reset AFU
status.cmd.code = PSL_JOB_RESET;
status.cmd.addr = 0;
status.cmd.request = AFU_REQUEST;
// FIXME: Add timeout
while (status.cmd.request != AFU_IDLE) short_delay();
// Read AFU descriptor
status.mmio.rnw = 1;
status.mmio.dw = 1;
status.mmio.desc = 1;
// Offset 0x00
status.mmio.addr = 0;
status.mmio.request = AFU_REQUEST;
// FIXME: Add timeout
while (status.mmio.request != AFU_IDLE) short_delay();
value = be64toh(status.mmio.data);
afu->desc.req_prog_model = value && 0xffff;
value >>= 16;
afu->desc.num_of_afu_CRs = value && 0xffff;
value >>= 16;
afu->desc.num_of_processes = value && 0xffff;
value >>= 16;
afu->desc.num_ints_per_process = value && 0xffff;
// Offset 0x20
status.mmio.addr = 8;
status.mmio.request = AFU_REQUEST;
// FIXME: Add timeout
while (status.mmio.request != AFU_IDLE) short_delay();
afu->desc.AFU_CR_len = be64toh(status.mmio.data);
// Offset 0x28
status.mmio.addr = 10;
status.mmio.request = AFU_REQUEST;
// FIXME: Add timeout
while (status.mmio.request != AFU_IDLE) short_delay();
afu->desc.AFU_CR_offset = be64toh(status.mmio.data);
// Offset 0x30
status.mmio.addr = 12;
status.mmio.request = AFU_REQUEST;
// FIXME: Add timeout
while (status.mmio.request != AFU_IDLE) short_delay();
afu->desc.PerProcessPSA = be64toh(status.mmio.data);
// Offset 0x38
status.mmio.addr = 14;
status.mmio.request = AFU_REQUEST;
// FIXME: Add timeout
while (status.mmio.request != AFU_IDLE) short_delay();
afu->desc.PerProcessPSA_offset = be64toh(status.mmio.data);
// Offset 0x40
status.mmio.addr = 16;
status.mmio.request = AFU_REQUEST;
// FIXME: Add timeout
while (status.mmio.request != AFU_IDLE) short_delay();
afu->desc.AFU_EB_len = be64toh(status.mmio.data);
// Offset 0x48
status.mmio.addr = 18;
status.mmio.request = AFU_REQUEST;
// FIXME: Add timeout
while (status.mmio.request != AFU_IDLE) short_delay();
afu->desc.AFU_EB_offset = be64toh(status.mmio.data);
status.mmio.desc = 0;
return afu;
}
char LCD_Putchar(char zeichen)
{
short_delay();
lcd_write_byte((char) zeichen);
return(1);
}
