/*
* Reports an error. reason should roughly give the reason for the error. Further
* details can be supplied in the message (which may contain null bytes and whose
* length is indicated by length). action describes which actions to take after the
* error has been reported.
*/
void error(err_reason_t reason, char *message, int length, err_action_t action) {
switch(action) {
case EA_RESUME:
dled_toggle();
#ifdef TRACE_ERRORS
dled_blink((int) reason);
debug_write(message, length);
#endif
return;
break;
case EA_RESET:
led_set_state(LED_STOP);
dled_blink((int) reason);
debug_write(message, length);
// Reset the system
SCB_AIRCR |= SCB_AIRCR_SYSRESETREQ;
break;
case EA_PANIC:
led_set_state(LED_STOP);
while (1) {
dled_blink((int) reason);
debug_write(message, length);
}
break;
}
}
void debug_writeBuf(ObjectDesc * self, ArrayDesc * arr, jint off, jint len)
{
#ifndef NO_DEBUG_OUT
#ifndef SMP
jint i;
#ifdef ALL_ARRAYS_32BIT
u4_t *field = (u4_t *) (arr->data);
#else
u1_t *field = (u1_t *) (arr->data);
#endif
DISABLE_IRQ;
for (i = off; i < off + len; i++) {
debug_write(self, field[i]);
}
RESTORE_IRQ;
#else /* use a spinlock to prevent trouble with other CPUs */
static spinlock_t printf_lock = SPIN_LOCK_UNLOCKED;
jint i;
#ifdef ALL_ARRAYS_32BIT
u4_t *field = (u4_t *) (arr->data);
#else
u1_t *field = (u1_t *) (arr->data);
#endif
DISABLE_IRQ;
spin_lock(&printf_lock);
for (i = off; i < off + len; i++) {
debug_write(self, field[i]);
}
spin_unlock(&printf_lock);
RESTORE_IRQ;
#endif
#endif /* NO_DEBUG_OUT */
}
void assert_failed(uint8_t* file, uint32_t line) {
debug_write("-assert_failed: file ");
debug_write((const char*) file);
debug_write(" on line ");
debug_write_u32(line, 10);
debug_write_line("");
/* Infinite loop */
while (1) {
}
}
static size_t gdb_consume_packet(uint8_t *data, size_t len)
{
size_t done;
debug(GDBSTUB_PKT, "consume (%D): [", len);
#ifdef CONFIG_GDBSTUB_PKT_DBG
debug_write(data, len);
debug_write((uint8_t*)"]\n", 2);
#endif
while(len)
{
switch(*data)
{
case GDB_ACK_BYTE:
if(!gdb_enabled())
{
debug(GDBSTUB_PKT, "gdb connect\n");
gdb_enable();
}
done = 1;
break;
case GDB_PKT_BYTE:
done = gdb_parse_packet(data, len);
if(!done)
goto __end;
break;
case GDB_INT_BYTE:
done = 1;
debug(GDBSTUB_PKT, "interrupt sequence requested\n");
gdb_interrupt_sequence();
break;
case GDB_NAK_BYTE:
done = 1;
gdb_ack();
break;
default:
done = 1;
debug(GDBSTUB_PKT, "gdb_stub unsupported '\\x%x' (sz %D)\n", *data, len);
gdb_unsupported();
break;
}
len -= done;
data += done;
}
__end:
return len;
}
void
debug_write_n_str (const char * str, int len)
{
int pos;
if (len > 0) {
for (pos = 0; pos < len; pos++)
debug_write (*str++);
} else {
while (*str) {
debug_write (*str++);
}
}
}
int sys_DebugPrint( const char *fmt, char **pzArgs )
{
if ( SysBase )
{
Thread_s *psThread = CURRENT_THREAD;
char String[512];
char zBuffer[1024];
uint32 nFlg;
nFlg = cli();
if ( NULL != psThread )
{
Process_s *psProc;
if ( ( psProc = CURRENT_PROC ) )
{
sprintf( String, "%d:%s::%s : %s", get_processor_id(), psProc->tc_zName, psThread->tr_zName, fmt );
}
else
{
sprintf( String, "%d:%s : %s", get_processor_id(), psThread->tr_zName, fmt );
}
}
else
{
sprintf( String, "%d : %s", get_processor_id(), fmt );
}
put_cpu_flags( nFlg );
sprintf( zBuffer, String, pzArgs[0], pzArgs[1], pzArgs[2], pzArgs[3], pzArgs[4], pzArgs[5], pzArgs[6], pzArgs[7], pzArgs[8], pzArgs[9], pzArgs[10], pzArgs[11], pzArgs[12] );
debug_write( zBuffer, strlen( zBuffer ) );
}
return ( 0 );
}
int main ( void )
{
utils_init();
debug_init();
pwm_in_init();
pwm_out_init();
_delay_ms(2000);
orientation_init();
char out[200];
float dt = 0;
uint32_t t = _micros();
uint32_t dt_c = 20000;
for ( ; ; )
{
t = _micros();
/* Read all data from sensor 1 */
orientation_update( 20.0 );
bzero( out , sizeof ( out ) );
/* Format data */
sprintf( out, "{\"x\":%f,\"y\":%f,\"z\":%f,\"xx\":%f,\"xy\":%f,\"xz\":%f}\n" , angles[PITCH], angles[ROLL], angles[YAW],acc_smooth[PITCH], acc_smooth[ROLL], acc_smooth[YAW]);
debug_write( out );
while((_micros() - t) < dt_c);
}
}
static ssize_t dbg_write(int fd, const void *buf, size_t count)
{
if (ps.opened)
if (ps.fd == fd)
return debug_write(ps.tid, (long *)buf, count);
return count;
}
/**
* @brief Creates a new UDP/IP socket
*
* Returns an integer describing a DGRAM (UDP) socket. The socket is automatically bound to some port.
*
* @param proto_osi3 is LIBSOCKET_IPv4 (AF_INET) or LIBSOCKET_IPv6 (AF_INET6).
* @param flags may be the flags specified in socket(2), i.e. SOCK_NONBLOCK and/or SOCK_CLOEXEC. More than one
* flags may be ORed. This argument is only sensible on Linux >= 2.6.27!
*
* @return The socket file descriptor number, on error -1.
*
* To send and receive data with this socket use the functions explained below, sendto_inet_dgram_socket() and recvfrom_inet_dgram_socket().
*/
int create_inet_dgram_socket(char proto_osi3, int flags)
{
int sfd;
if (proto_osi3 != LIBSOCKET_IPv4 && proto_osi3 != LIBSOCKET_IPv6)
{
# ifdef VERBOSE
debug_write("create_inet_dgram_socket: osi3 argument invalid for DGRAM sockets\n");
# endif
return -1;
}
switch ( proto_osi3 )
{
case LIBSOCKET_IPv4 :
sfd = socket(AF_INET,SOCK_DGRAM|flags,0);
break;
case LIBSOCKET_IPv6 :
sfd = socket(AF_INET6,SOCK_DGRAM|flags,0);
break;
default:
return -1;
}
if ( -1 == check_error(sfd) )
return -1;
return sfd;
}
/**
* @brief Create a UNIX DGRAM socket
*
* @param bind_path If not `NULL`, bind to `bind_path`.
* @param flags Flags to pass to `socket(2)` (varies from OS to OS; look in the man pages)
*
* @retval >0 Success. Value is socket.
* @retval <0 Error.
*/
int create_unix_dgram_socket(const char* bind_path, int flags)
{
int sfd, retval;
struct sockaddr_un saddr;
if ( -1 == check_error(sfd = socket(AF_UNIX,SOCK_DGRAM|flags,0)) )
return -1;
memset(&saddr,0,sizeof(struct sockaddr_un));
if ( bind_path != NULL )
{
if ( (retval = unlink(bind_path)) == -1 && errno != ENOENT ) // If there's another error than "doesn't exist"
{
check_error(retval);
return -1;
}
if ( strlen(bind_path) > (sizeof(saddr.sun_path)-1) )
{
# ifdef VERBOSE
debug_write("create_unix_dgram_socket: UNIX socket path too long\n");
# endif
return -1;
}
saddr.sun_family = AF_UNIX;
strncpy(saddr.sun_path,bind_path,sizeof(saddr.sun_path));
bind(sfd,(struct sockaddr*)&saddr,sizeof(struct sockaddr));
}
return sfd;
}
/**
* @brief Create and connect a new UNIX STREAM socket.
*
* Creates and connects a new STREAM socket with the socket given in `path`.
*
* @retval >0 Success; return value is a socket file descriptor
* @retval <0 Error.
*/
int create_unix_stream_socket(const char* path, int flags)
{
struct sockaddr_un saddr;
int sfd;
if ( path == NULL )
return -1;
if ( -1 == check_error(sfd = socket(AF_UNIX,SOCK_STREAM|flags,0)) )
return -1;
memset(&saddr,0,sizeof(struct sockaddr_un));
if ( strlen(path) > (sizeof(saddr.sun_path)-1) )
{
# ifdef VERBOSE
debug_write("create_unix_stream_socket: UNIX socket destination path too long\n");
# endif
return -1;
}
saddr.sun_family = AF_UNIX;
strncpy(saddr.sun_path,path,sizeof(saddr.sun_path)-1);
if ( -1 == check_error(connect(sfd,(struct sockaddr*)&saddr,sizeof saddr)) )
return -1;
return sfd;
}
void on_usart1_irq() {
char line[MAX_LINE_LENGTH];
if (USART_GetITStatus(DEBUG_USART, USART_IT_RXNE) != RESET) {
uint8_t data[1];
data[0] = USART_ReceiveData(DEBUG_USART);
ring_buffer_u8_write(&g_usartInputRingBuffer, data, 1);
while (ring_buffer_u8_readline(&g_usartInputRingBuffer, line, MAX_LINE_LENGTH) > 0) {
if(strcmp(line, "!CONNECT\n") == 0) {
debug_write_line("+OK");
debug_write_line("!clear");
debug_write_line("!set name,stm32-wifi-ir");
debug_write_line("!set description,'IR TX/RX over WiFi'");
debug_write_line("?add widgets");
debug_write_line("!add label,code,1,0,1,1");
debug_write_line("!code.set minWidth,150");
debug_write_line("!code.set title,'Code'");
debug_write_line("!code.set text,'xxxxxxxxxxxxxxxxxxxx'");
} else if(strncmp(line, "!TX", 3) == 0) {
debug_write_line("+OK");
IrCode* code = ir_code_getByIndex(0);
ir_tx_send(code);
} else {
debug_write("?Unknown command: ");
debug_write_line(line);
}
}
}
}
size_t __vprintf(const char *format, va_list params)
{
size_t retval;
retval = __vsnprintf(vprint_buffer, sizeof(vprint_buffer), format, params);
debug_write((uint8_t*)vprint_buffer, retval-1);
return retval;
}
void vdebug(int level, const char* fmt, va_list args)
{
if (level > DebugLevel) return;
char buffer[256]; buffer[255] = '\0';
vsnprintf(buffer, 255, fmt, args);
debug_write(buffer, strlen(buffer));
}
int _write_r(struct _reent *re, int fd, const void *data, size_t len) {
if (fd == 1 || fd == 2) {
debug_write(data, len);
return len;
}
return -1;
}
END_TEST
START_TEST(test_debug) {
const char *errstr = NULL;
int ostdout, spair[2];
ssize_t len;
struct pcap_file_header pcap_fhdr = {};
struct parent_msg msg = {};
char buf[2048];
mark_point();
if (isatty(STDOUT_FILENO)) {
errstr = "please redirect stdout to tcpdump or a file";
my_log(CRIT, "check");
WRAP_FATAL_START();
debug_header();
WRAP_FATAL_END();
fail_unless (strncmp(check_wrap_errstr, errstr, strlen(errstr)) == 0,
"incorrect message logged: %s", check_wrap_errstr);
}
ostdout = dup(STDOUT_FILENO);
my_socketpair(spair);
dup2(spair[0], STDOUT_FILENO);
close(ostdout);
errstr = "check";
my_log(CRIT, errstr);
mark_point();
debug_header();
fflush(stdout);
fail_unless (strncmp(check_wrap_errstr, errstr, strlen(errstr)) == 0,
"incorrect message logged: %s", check_wrap_errstr);
len = read(spair[1], &pcap_fhdr, sizeof(pcap_fhdr));
fail_unless(len == sizeof(pcap_fhdr),
"failed to read pcap header");
fail_unless(pcap_fhdr.magic == PCAP_MAGIC,
"invalid pcap header returned");
fail_unless(pcap_fhdr.snaplen == ETHER_MAX_LEN,
"invalid pcap header returned");
fail_unless(pcap_fhdr.linktype == DLT_EN10MB,
"invalid pcap header returned");
mark_point();
msg.len = ETHER_MIN_LEN;
debug_write(&msg, 0);
fflush(stdout);
fail_unless (strncmp(check_wrap_errstr, errstr, strlen(errstr)) == 0,
"incorrect message logged: %s", check_wrap_errstr);
len = read(spair[1], buf, sizeof(buf));
fail_unless(len == (PCAP_PKTHDR_SIZ + msg.len),
"failed to read pcap record");
close(spair[0]);
close(spair[1]);
dup2(ostdout, STDOUT_FILENO);
close(ostdout);
}
/*
* FUNCTION: pp_close
* ------------------
* closes the pretty print output file
*
* returns: 0 on successful completion, -1 on failure
*/
int
pp_close(){
if (fclose(parserOutput) != 0)
{
debug_write("failed to close parser output file\n");
return -1;
}
return 0;
}
/*
* FUNCTION: pp_open
* -----------------
* creates and opens a pretty print output file
*
* filename: filename string for file creation
*
* returns: 0 on successful completion, -1 on failure
*/
int
open_parser_output(char *filename){
if ((parserOutput = fopen(filename, "w")) == NULL)
{
debug_write("failed to open parser output file\n");
return -1;
}
return 0;
}
/* Helper function to display text to the debug log in Codescape
* Returns: None */
void DebugOut( const char *p_pString )
{
#if defined DEBUG
if( p_pString != NULL )
{
debug_write( SNASM_STDOUT, p_pString, strlen( p_pString ) );
}
#endif
}
/**
* @brief This function is the equivalent to `sendto(2)`
*
* @param sfd is the *Socket File Descriptor* (every socket file descriptor argument in libsocket is called sfd) which
* you got from create_inet_dgram_socket(). *The usage with STREAM sockets is not recommended and the result is undefined!*
* @param buf is a pointer to some data.
* @param size is the length of the buffer to which buf points.
* @param host is the host to which we want to send the data. It's a string so you may specify everything what's resolved by
* getaddrinfo(), i.e. an IP ("193.21.34.21") or a hostname ("example.net").
* @param service is the port on the remote host. Like in host, you may specify the port either as number ("123") or as service string ("ntp", "http", "gopher").
* @param sendto_flags is available on all platforms. The value given here goes directly to the internal sendto() call. The flags which may be specified differ between the
* platforms.
*
* If it is not possible to send data at the moment, this call blocks excepted you specified SOCK_NONBLOCK when creating the socket.
*
* @retval n *n* bytes of data could be sent.
* @retval -1 Error.
*/
ssize_t sendto_inet_dgram_socket(int sfd, const void* buf, size_t size, const char* host, const char* service, int sendto_flags)
{
struct sockaddr_storage oldsock;
struct addrinfo *result, *result_check, hint;
socklen_t oldsocklen = sizeof(struct sockaddr_storage);
int return_value;
# ifdef VERBOSE
const char* errstring;
# endif
if ( sfd < 0 )
return -1;
if ( buf == NULL )
return -1;
if ( size == 0 )
return 0;
if ( host == NULL || service == NULL )
return -1;
if ( -1 == check_error(getsockname(sfd,(struct sockaddr*)&oldsock,(socklen_t*)&oldsocklen)) )
return -1;
memset(&hint,0,sizeof(struct addrinfo));
/*
* This works for Linux > 2.6.32
socklen_t dom_len = sizeof(hint.ai_family);
getsockopt(sfd,SOL_SOCKET,SO_DOMAIN,&hint.ai_family,&dom_len);
*/
hint.ai_family = oldsock.ss_family;
hint.ai_socktype = SOCK_DGRAM;
if ( 0 != (return_value = getaddrinfo(host,service,&hint,&result)))
{
# ifdef VERBOSE
errstring = gai_strerror(return_value);
debug_write(errstring);
# endif
return -1;
}
for ( result_check = result; result_check != NULL; result_check = result_check->ai_next ) // go through the linked list of struct addrinfo elements
{
if ( -1 != (return_value = sendto(sfd,buf,size,sendto_flags,result_check->ai_addr,result_check->ai_addrlen))) // connected without error
{
break; // Exit loop if send operation was successful
} else
{
check_error(return_value);
}
}
return return_value;
}
/**
* FUNCTION: buffer_free
* ---------------------
* frees memory allocated to buffer
*/
void
buffer_free(Buffer * buffer){
int i = 0;
for (i = 0; i < BUFFER_SIZE; i++)
{
req_free(&buffer->requests[i]);
}
debug_write("free requests pointer\n");
free(buffer->requests);
}
void local_debug(char *args,...)
{
char szline[DEF_MAX_LOGLINE_LEN] = {0};
va_list pArgList ;
va_start(pArgList, args);
vsprintf(szline, args, pArgList) ;
va_end(pArgList);
debug_write(DEF_LOGFILE_LOCAL_NAME, szline);
}
/**
* @brief Create a passive UNIX socket
*
* Creating a DGRAM server socket is the same as creating one
* using `create_unix_dgram_socket()` but with latter you may
* also not bind to anywhere.
*
* @param path Path to bind the socket to
* @param socktype `LIBSOCKET_STREAM` or `LIBSOCKET_DGRAM`
* @param flags Flags for `socket(2)`.
*
* @retval >0 Success; returned value is a file descriptor for the socket
* @retval <0 An error occurred.
*/
int create_unix_server_socket(const char* path, int socktype, int flags)
{
struct sockaddr_un saddr;
int sfd, type, retval;
if ( path == NULL )
return -1;
if ( strlen(path) > (sizeof(saddr.sun_path)-1) )
{
# ifdef VERBOSE
debug_write("create_unix_server_socket: Path too long\n",14);
# endif
return -1;
}
switch ( socktype )
{
case LIBSOCKET_STREAM:
type = SOCK_STREAM;
break;
case LIBSOCKET_DGRAM:
type = SOCK_DGRAM;
break;
default:
return -1;
}
if ( -1 == check_error(sfd = socket(AF_UNIX,type|flags,0)) )
return -1;
if ( (retval = unlink(path)) == -1 && errno != ENOENT ) // If there's another error than "doesn't exist"
{
check_error(retval);
return -1;
}
memset(&saddr,0,sizeof(struct sockaddr_un));
saddr.sun_family = AF_UNIX;
strncpy(saddr.sun_path,path,sizeof(saddr.sun_path) - 1);
if ( -1 == check_error(bind(sfd,(struct sockaddr*)&saddr,sizeof(struct sockaddr_un))) )
return -1;
if ( type == SOCK_STREAM )
{
if ( -1 == check_error(listen(sfd,LIBSOCKET_BACKLOG)))
return -1;
}
return sfd;
}
void debug_free(void *ptr)
{
if ( ptr == NULL )
{
debug_write( "debug_free() : Pointer is NULL\n" );
return;
}
ptr = ((char *)ptr) - sizeof(size_t);
debug_counter( "mem_alloc", -(*((size_t *)ptr)) );
free( ptr );
}
void loop() {
IrRecv* irRecv = ir_rx_recv();
if(irRecv != NULL) {
IrCode* code = ir_code_decode(irRecv->buffer, irRecv->bufferLength);
debug_write("?rx: ");
if(code != NULL) {
debug_write_u16(code->brand, 16);
debug_write_u16(code->key, 16);
} else {
debug_write("(");
debug_write_u16(irRecv->bufferLength, 10);
debug_write("): ");
for(int i = 0; i < irRecv->bufferLength; i++) {
debug_write_u16(irRecv->buffer[i], 10);
debug_write(",");
}
}
debug_write_line("");
}
}
void DBGLIBENTRY debug_state()
{
PDBGCOUNTER pScan;
debug_write( "Debug counters:\n"
"Counter name\tIncr. times\tDecr. times\tValue\n" );
for( pScan = pCounters; pScan != NULL; pScan = pScan->pNext )
debug_text( "%-17s %5u %5u(%d) %5d\n",
pScan->pcName, pScan->cInc, pScan->cDec,
pScan->cInc - pScan->cDec, pScan->iValue );
}
void Maximize_method::read(vector <string> words,
unsigned int & pos,
Program_options & options) {
allocate(options.systemtext[0], sys);
sys->generatePseudo(options.pseudotext, pseudo);
debug_write(cout, "wfdata allocate\n");
wfdata=NULL;
if(options.twftext.size() < 1) error("Need TRIALFUNC section for OPTIMIZE");
allocate(options.twftext[0], sys, wfdata);
}
static size_t gdb_parse_packet(uint8_t *data, size_t len)
{
uint8_t *ptr;
size_t dlen;
if(len < GDB_PKT_SZ)
{
debug(GDBSTUB_PKT, "packet too small\n");
return 0;
}
data++;
ptr = data;
dlen = len - 1;
while(dlen && *ptr != GDB_END_BYTE)
{
dlen--;
ptr++;
}
if(dlen < (GDB_PKT_SZ-1))
{
debug(GDBSTUB_PKT, "incorrect packet: len %D, dlen %D [", len, dlen);
#ifdef CONFIG_GDBSTUB_PKT_DBG
data--;
debug_write(data, len);
#endif
debug(GDBSTUB_PKT, "]\n");
return 0;
}
dlen = (size_t)(ptr - data);
if(dlen)
{
ptr++;
if(gdb_checksum_verify(data, dlen, ptr))
{
debug(GDBSTUB_PKT, "cmd packet (%D)\n", dlen);
__gdb_reset_buffer();
gdb_process_packet(data, dlen);
}
else
{
debug(GDBSTUB_PKT, "invalid checksum\n");
gdb_nak();
}
}
return dlen+GDB_PKT_SZ;
}
/**
* FUNCTION: req_free
* ------------------
* frees individual fields of provided request
*
* request: Request structure to free
*/
void
req_free(Request *request)
{
debug_write("freeing request structure field memory\n");
free(request->host);
free(request->clientId);
free(request->userId);
free(request->strTime);
free(request->req);
free(request->referer);
free(request->userAgent);
}
/*!
*/
void Postprocess_method::read(vector <string> words,
unsigned int & pos,
Program_options & options)
{
if(!readvalue(words, pos=0, configfile, "READCONFIG"))
error("Need READCONFIG in Postprocess");
if(!readvalue(words, pos=0, nskip, "NSKIP"))
nskip=0;
evaluate_energy=true;
if(haskeyword(words,pos=0,"NOENERGY")) evaluate_energy=false;
vector <vector < string> > dens_words;
vector<string> tmp_dens;
pos=0;
while(readsection(words, pos, tmp_dens, "DENSITY")) {
dens_words.push_back(tmp_dens);
}
vector <vector < string> > avg_words;
pos=0;
while(readsection(words, pos, tmp_dens, "AVERAGE")) {
avg_words.push_back(tmp_dens);
}
sys=NULL;
allocate(options.systemtext[0], sys);
sys->generatePseudo(options.pseudotext, pseudo);
debug_write(cout, "wfdata allocate\n");
wfdata=NULL;
if(options.twftext.size() < 1) error("Need TRIALFUNC section for POSTPROCESS");
allocate(options.twftext[0], sys, wfdata);
average_var.Resize(avg_words.size());
average_var=NULL;
for(int i=0; i< average_var.GetDim(0); i++) {
allocate(avg_words[i], sys, wfdata, average_var(i));
}
densplt.Resize(dens_words.size());
for(int i=0; i< densplt.GetDim(0); i++) {
allocate(dens_words[i], sys, options.runid,densplt(i));
}
}
