348 lines
10 KiB
C++
348 lines
10 KiB
C++
/*
|
|
*/
|
|
|
|
#include "../../include/RTSCoproHelper.h"
|
|
#include <signal.h>
|
|
#include <stdio.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <termios.h>
|
|
#include <fcntl.h>
|
|
#include <inttypes.h>
|
|
#include <time.h>
|
|
#include <pthread.h>
|
|
#include <unistd.h> // for usleep
|
|
#include <math.h>
|
|
#include <sys/ioctl.h>
|
|
#include <sys/time.h>
|
|
#include <sys/mman.h>
|
|
#include <sys/stat.h>
|
|
#include <sys/types.h>
|
|
#include <sys/eventfd.h>
|
|
#include <sys/poll.h>
|
|
#include <regex.h>
|
|
#include <sched.h>
|
|
#include <assert.h>
|
|
#include <errno.h>
|
|
#include <error.h>
|
|
|
|
|
|
|
|
#define MAX_BUF 80
|
|
|
|
#define TTY_CTRL_OPTS (CS8 | CLOCAL | CREAD)
|
|
#define TTY_INPUT_OPTS IGNPAR
|
|
#define TTY_OUTPUT_OPTS 0
|
|
#define TTY_LOCAL_OPTS 0
|
|
|
|
#define CMDCHANNEL 0
|
|
#define DATACHANNEL 1
|
|
|
|
RTSCoproHelper::RTSCoproHelper(/* args */)
|
|
{
|
|
}
|
|
|
|
|
|
|
|
|
|
RTSCoproHelper::~RTSCoproHelper()
|
|
{
|
|
}
|
|
|
|
int RTSCoproHelper::Init(std::string fwPath, std::string fwName) {
|
|
int res = -1;
|
|
|
|
if (this->Copro_isFwRunning() != 0){
|
|
//copro is already Runnibng nothing TODO
|
|
//res = this->Copro_openTtyRpmsg(CMDCHANNEL, 0);
|
|
//this->Copro_stopFw();
|
|
//return res;
|
|
return 0;
|
|
}
|
|
|
|
//load Firmware
|
|
//set firmware path
|
|
res = this->Copro_setFwPath(fwPath.c_str());
|
|
if (res >= 0) {
|
|
printf("CA7 : Success setting the fwpath, res=%d\n", res);
|
|
res = this->Copro_setFwName(fwName.c_str());
|
|
if (res >= 0) {
|
|
printf("CA7 : Success setting the fwname, res=%d\n", res);
|
|
res = this->Copro_startFw();
|
|
if (res >= 0) {
|
|
printf("CA7 : Success starting Copro, err=%d\n", res);
|
|
res = this->Copro_openTtyRpmsg(CMDCHANNEL, 1);
|
|
if (res >= 0 ){
|
|
printf("CA7 : Success opening Cmdchannel, err=%d\n", res);
|
|
|
|
}
|
|
else {
|
|
printf("CA7 : Error opening Cmdchannel, err=%d\n", res);
|
|
}
|
|
}
|
|
else {
|
|
printf("CA7 : Error could not start the firmware, res=-%d\n", res);
|
|
}
|
|
}
|
|
else {
|
|
printf("CA7 : Error setting the fwName, err=-%d\n", res);
|
|
}
|
|
}
|
|
else {
|
|
printf("CA7 : Error setting the fwpath, err=-%d\n", res);
|
|
}
|
|
return res;
|
|
}
|
|
|
|
|
|
/********************************************************************************
|
|
Copro functions allowing to manage a virtual TTY over RPMSG
|
|
*********************************************************************************/
|
|
int RTSCoproHelper::Copro_isFwRunning(void)
|
|
{
|
|
int fd;
|
|
size_t byte_read;
|
|
bool result = 0;
|
|
unsigned char bufRead[MAX_BUF];
|
|
// char *user = getenv("USER");
|
|
// if (user && strncmp(user, "root", 4)) {
|
|
// system("XTERM=xterm su root -c 'cat /sys/class/remoteproc/remoteproc0/state' > /tmp/remoteproc0_state");
|
|
// fd = open("/tmp/remoteproc0_state", O_RDONLY);
|
|
// } else {
|
|
fd = open("/sys/class/remoteproc/remoteproc0/state", O_RDONLY);
|
|
// }
|
|
if (fd < 0) {
|
|
printf("CA7 : Error opening remoteproc0/state, err=-%d\n", errno);
|
|
return (errno * -1);
|
|
}
|
|
byte_read = (size_t) read (fd, bufRead, MAX_BUF);
|
|
if (byte_read >= strlen("running")) {
|
|
char* pos = strstr((char*)bufRead, "running");
|
|
if(pos) {
|
|
result = 1;
|
|
}
|
|
}
|
|
close(fd);
|
|
return result;
|
|
}
|
|
|
|
int RTSCoproHelper::Copro_stopFw(void)
|
|
{
|
|
int fd;
|
|
// char *user = getenv("USER");
|
|
// if (user && strncmp(user, "root",4)) {
|
|
// system("su root -c 'echo stop > /sys/class/remoteproc/remoteproc0/state'");
|
|
// return 0;
|
|
// }
|
|
fd = open("/sys/class/remoteproc/remoteproc0/state", O_RDWR);
|
|
if (fd < 0) {
|
|
printf("CA7 : Error opening remoteproc0/state, err=-%d\n", errno);
|
|
return (errno * -1);
|
|
}
|
|
write(fd, "stop", strlen("stop"));
|
|
close(fd);
|
|
return 0;
|
|
}
|
|
|
|
int RTSCoproHelper::Copro_startFw(void)
|
|
{
|
|
int fd;
|
|
// char *user = getenv("USER");
|
|
// if (user && strncmp(user, "root",4)) {
|
|
// system("su root -c 'echo start > /sys/class/remoteproc/remoteproc0/state'");
|
|
// return 0;
|
|
// }
|
|
fd = open("/sys/class/remoteproc/remoteproc0/state", O_RDWR);
|
|
if (fd < 0) {
|
|
printf("CA7 : Error opening remoteproc0/state, err=-%d\n", errno);
|
|
return (errno * -1);
|
|
}
|
|
write(fd, "start", strlen("start"));
|
|
close(fd);
|
|
return 0;
|
|
}
|
|
|
|
int RTSCoproHelper::Copro_getFwPath(char* pathStr)
|
|
{
|
|
int fd;
|
|
int byte_read;
|
|
// char *user = getenv("USER");
|
|
// if (user && strncmp(user, "root",4)) {
|
|
// system("XTERM=xterm su root -c 'cat /sys/module/firmware_class/parameters/path' > /tmp/parameters_path");
|
|
// fd = open("/tmp/parameters_path", O_RDONLY);
|
|
// } else {
|
|
fd = open("/sys/module/firmware_class/parameters/path", O_RDONLY);
|
|
// }
|
|
if (fd < 0) {
|
|
printf("CA7 : Error opening firmware_class/parameters/path, err=-%d\n", errno);
|
|
return (errno * -1);
|
|
}
|
|
byte_read = read (fd, pathStr, MAX_BUF);
|
|
close(fd);
|
|
return byte_read;
|
|
}
|
|
|
|
int RTSCoproHelper::Copro_setFwPath(const char* pathStr)
|
|
{
|
|
int fd;
|
|
int result = 0;
|
|
// char *user = getenv("USER");
|
|
// if (user && strncmp(user, "root",4)) {
|
|
// char cmd[1024];
|
|
// snprintf(cmd, 1024, "echo %s > /sys/module/firmware_class/parameters/path", pathStr);
|
|
// //snprintf(cmd, 1024, "su root -c 'echo %s > /sys/module/firmware_class/parameters/path'", pathStr);
|
|
// system(cmd);
|
|
// return strlen(pathStr);
|
|
// }
|
|
fd = open("/sys/module/firmware_class/parameters/path", O_RDWR);
|
|
if (fd < 0) {
|
|
printf("CA7 : Error opening firmware_class/parameters/path, err=-%d\n", errno);
|
|
return (errno * -1);
|
|
}
|
|
result = write(fd, pathStr, strlen(pathStr));
|
|
close(fd);
|
|
return result;
|
|
}
|
|
|
|
int RTSCoproHelper::Copro_getFwName(char* pathStr)
|
|
{
|
|
int fd;
|
|
int byte_read;
|
|
// char *user = getenv("USER");
|
|
// if (user && strncmp(user, "root",4)) {
|
|
// system("XTERM=xterm su root -c 'cat /sys/class/remoteproc/remoteproc0/firmware' > /tmp/remoteproc0_firmware");
|
|
// fd = open("/tmp/remoteproc0_firmware", O_RDONLY);
|
|
// } else {
|
|
fd = open("/sys/class/remoteproc/remoteproc0/firmware", O_RDWR);
|
|
// }
|
|
if (fd < 0) {
|
|
printf("CA7 : Error opening remoteproc0/firmware, err=-%d\n", errno);
|
|
return (errno * -1);
|
|
}
|
|
byte_read = read (fd, pathStr, MAX_BUF);
|
|
close(fd);
|
|
return byte_read;
|
|
}
|
|
|
|
int RTSCoproHelper::Copro_setFwName(const char* nameStr)
|
|
{
|
|
int fd;
|
|
int result = 0;
|
|
// char *user = getenv("USER");
|
|
// if (user && strncmp(user, "root",4)) {
|
|
// char cmd[1024];
|
|
// snprintf(cmd, 1024, "su root -c 'echo %s > /sys/class/remoteproc/remoteproc0/firmware'", nameStr);
|
|
// system(cmd);
|
|
// return strlen(nameStr);
|
|
// }
|
|
fd = open("/sys/class/remoteproc/remoteproc0/firmware", O_RDWR);
|
|
if (fd < 0) {
|
|
printf("CA7 : Error opening remoteproc0/firmware, err=-%d\n", errno);
|
|
return (errno * -1);
|
|
}
|
|
result = write(fd, nameStr, strlen(nameStr));
|
|
close(fd);
|
|
return result;
|
|
}
|
|
|
|
int RTSCoproHelper::Copro_openTtyRpmsg(int ttyNb, [[maybe_unused]]int modeRaw)
|
|
{
|
|
|
|
//struct termios tiorpmsg;
|
|
char devName[50];
|
|
sprintf(devName, "/dev/ttyRPMSG%d", ttyNb%2);
|
|
mFdRpmsg[ttyNb%2] = open(devName, O_RDWR | O_NOCTTY | O_NONBLOCK);
|
|
// mFdRpmsg[ttyNb%2] = open("rpmsg-client-sample", O_RDWR | O_NOCTTY | O_NONBLOCK);
|
|
if (mFdRpmsg[ttyNb%2] < 0) {
|
|
printf("CA7 : Error opening ttyRPMSG%d, err=-%d\n", ttyNb%2, errno);
|
|
return (errno * -1);
|
|
}
|
|
printf("CA7 : Success opening ttyRPMSG%d \n", ttyNb%2);
|
|
// #if 1
|
|
// /* get current port settings */
|
|
// tcgetattr(mFdRpmsg[ttyNb%2],&tiorpmsg);
|
|
// if (modeRaw) {
|
|
// #if 0
|
|
// tiorpmsg.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP
|
|
// | INLCR | IGNCR | ICRNL | IXON);
|
|
// tiorpmsg.c_oflag &= ~OPOST;
|
|
// tiorpmsg.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
|
|
// tiorpmsg.c_cflag &= ~(CSIZE | PARENB);
|
|
// tiorpmsg.c_cflag |= CS8;
|
|
// #else
|
|
// memset(&tiorpmsg, 0, sizeof(tiorpmsg));
|
|
// tiorpmsg.c_cflag = TTY_CTRL_OPTS;
|
|
// tiorpmsg.c_iflag = TTY_INPUT_OPTS;
|
|
// tiorpmsg.c_oflag = TTY_OUTPUT_OPTS;
|
|
// tiorpmsg.c_lflag = TTY_LOCAL_OPTS;
|
|
// tiorpmsg.c_cc[VTIME] = 0;
|
|
// tiorpmsg.c_cc[VMIN] = 1;
|
|
// cfmakeraw(&tiorpmsg);
|
|
// #endif
|
|
// } else {
|
|
// /* ECHO off, other bits unchanged */
|
|
// tiorpmsg.c_lflag &= ~ECHO;
|
|
// /*do not convert LF to CR LF */
|
|
// tiorpmsg.c_oflag &= ~ONLCR;
|
|
// }
|
|
// if (tcsetattr(mFdRpmsg[ttyNb%2], TCSANOW, &tiorpmsg) < 0) {
|
|
// printf("Error %d in copro_openTtyRpmsg(%d) tcsetattr\n", errno, ttyNb);
|
|
// return (errno * -1);
|
|
// }
|
|
// #endif
|
|
return 0;
|
|
}
|
|
|
|
int RTSCoproHelper::Copro_closeTtyRpmsg(int ttyNb)
|
|
{
|
|
close(mFdRpmsg[ttyNb%2]);
|
|
mFdRpmsg[ttyNb%2] = -1;
|
|
return 0;
|
|
}
|
|
|
|
int RTSCoproHelper::Copro_writeTtyRpmsg(int ttyNb, int len, char* pData)
|
|
{
|
|
int result = 0;
|
|
if (mFdRpmsg[ttyNb%2] < 0) {
|
|
printf("CA7 : Error writing ttyRPMSG%d, fileDescriptor is not set\n", ttyNb%2);
|
|
return mFdRpmsg[ttyNb%2];
|
|
}
|
|
|
|
result = write(mFdRpmsg[ttyNb%2], pData, len);
|
|
if (result >= 0) {
|
|
printf("CA7 : Succes writing ttyRPMSG%d, result=%d\n", ttyNb%2, result);
|
|
}
|
|
else {
|
|
printf("CA7 : Error writing ttyRPMSG%d, err=-%d\n", ttyNb%2, result);
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
int RTSCoproHelper::Copro_readTtyRpmsg(int ttyNb, int len, char* pData)
|
|
{
|
|
int byte_rd, byte_avail;
|
|
int result = 0;
|
|
if (mFdRpmsg[ttyNb%2] < 0) {
|
|
printf("CA7 : Error reading ttyRPMSG%d, fileDescriptor is not set\n", ttyNb%2);
|
|
return mFdRpmsg[ttyNb%2];
|
|
}
|
|
ioctl(mFdRpmsg[ttyNb%2], FIONREAD, &byte_avail);
|
|
if (byte_avail > 0) {
|
|
if (byte_avail >= len) {
|
|
byte_rd = read (mFdRpmsg[ttyNb%2], pData, len);
|
|
} else {
|
|
byte_rd = read (mFdRpmsg[ttyNb%2], pData, byte_avail);
|
|
}
|
|
//printf("CA7 : read successfully %d bytes to %p, [0]=0x%x\n", byte_rd, pData, pData[0]);
|
|
result = byte_rd;
|
|
} else {
|
|
result = 0;
|
|
}
|
|
return result;
|
|
}
|
|
/********************************************************************************
|
|
End of Copro functions
|
|
*********************************************************************************/
|