src/main/common/log.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * INAV is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <stdarg.h>
  21 #include <ctype.h>
  22
  23 #if defined(SEMIHOSTING)
  24 #include <stdio.h>
  25 #endif
  26
  27 #include "build/version.h"
  28
  29 #include "drivers/serial.h"
  30 #include "drivers/time.h"
  31
  32 #include "common/log.h"
  33 #include "common/printf.h"
  34 #include "common/utils.h"
  35
  36 #include "config/feature.h"
  37 #include "config/parameter_group_ids.h"
  38
  39 #include "io/serial.h"
  40
  41 #include "fc/config.h"
  42
  43 #include "msp/msp.h"
  44 #include "msp/msp_serial.h"
  45 #include "msp/msp_protocol.h"
  46
  47 #if defined(USE_LOG)
  48
  49 #define LOG_PREFIX                  "[%6d.%03d] "
  50 #define LOG_PREFIX_FORMATTED_SIZE   13
  51
  52 static serialPort_t * logPort = NULL;
  53 static mspPort_t * mspLogPort = NULL;
  54
  55 PG_REGISTER(logConfig_t, logConfig, PG_LOG_CONFIG, 0);
  56
  57 void logInit(void)
  58 {
  59     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_LOG);
  60     if (!portConfig) {
  61         return;
  62     }
  63
  64     bool portIsSharedWithMSP = false;
  65
  66     if (determinePortSharing(portConfig, FUNCTION_LOG) == PORTSHARING_SHARED) {
  67         // We support sharing a LOG port only with MSP
  68         if (portConfig->functionMask != (FUNCTION_LOG | FUNCTION_MSP)) {
  69             return;
  70         }
  71         portIsSharedWithMSP = true;
  72     }
  73
  74     // If the port is shared with MSP, reuse the port
  75     if (portIsSharedWithMSP) {
  76         const serialPort_t *logAndMspPort = findSharedSerialPort(FUNCTION_LOG, FUNCTION_MSP);
  77         if (!logAndMspPort) {
  78             return;
  79         }
  80
  81         mspLogPort = mspSerialPortFind(logAndMspPort);
  82         if (!mspLogPort) {
  83             return;
  84         }
  85
  86     } else {
  87         logPort = openSerialPort(portConfig->identifier, FUNCTION_LOG, NULL, NULL, baudRates[BAUD_921600], MODE_TX, SERIAL_NOT_INVERTED);
  88         if (!logPort) {
  89             return;
  90         }
  91     }
  92     // Initialization done
  93     LOG_I(SYSTEM, "%s/%s %s %s / %s (%s)",
  94         FC_FIRMWARE_NAME,
  95         targetName,
  96         FC_VERSION_STRING,
  97         buildDate,
  98         buildTime,
  99         shortGitRevision
 100     );
 101 }
 102
 103 static void logPutcp(void *p, char ch)
 104 {
 105     *(*((char **) p))++ = ch;
 106 }
 107
 108 static void logPrint(const char *buf, size_t size)
 109 {
 110 #if defined(SEMIHOSTING)
 111     static bool semihostingInitialized = false;
 112     extern void initialise_monitor_handles(void);
 113
 114     if (!semihostingInitialized) {
 115         initialise_monitor_handles();
 116         semihostingInitialized = true;
 117     }
 118     for (size_t ii = 0; ii < size; ii++) {
 119         fputc(buf[ii], stdout);
 120     }
 121 #endif
 122     if (logPort) {
 123         // Send data via UART (if configured & connected - a safeguard against zombie VCP)
 124         if (serialIsConnected(logPort)) {
 125             serialPrint(logPort, buf);
 126         }
 127     } else if (mspLogPort) {
 128         mspSerialPushPort(MSP_DEBUGMSG, (uint8_t*)buf, size, mspLogPort, MSP_V2_NATIVE);
 129     }
 130 }
 131
 132 static size_t logFormatPrefix(char *buf, const timeMs_t timeMs)
 133 {
 134     // Write timestamp
 135     return tfp_sprintf(buf, LOG_PREFIX, (int)(timeMs / 1000), (int)(timeMs % 1000));
 136 }
 137
 138 static bool logHasOutput(void)
 139 {
 140 #if defined(SEMIHOSTING)
 141     return true;
 142 #else
 143     return logPort || mspLogPort;
 144 #endif
 145 }
 146
 147 static bool logIsEnabled(logTopic_e topic, unsigned level)
 148 {
 149     return logHasOutput() && (level <= logConfig()->level || (logConfig()->topics & (1 << topic)));
 150 }
 151
 152 void _logf(logTopic_e topic, unsigned level, const char *fmt, ...)
 153 {
 154     char buf[128];
 155     char *bufPtr;
 156     int charCount;
 157
 158     STATIC_ASSERT(MSP_PORT_OUTBUF_SIZE >= sizeof(buf), MSP_PORT_OUTBUF_SIZE_not_big_enough_for_log);
 159
 160     if (!logIsEnabled(topic, level)) {
 161         return;
 162     }
 163
 164     charCount = logFormatPrefix(buf, millis());
 165     bufPtr = &buf[charCount];
 166
 167     // Write message
 168     va_list va;
 169     va_start(va, fmt);
 170     charCount += tfp_format(&bufPtr, logPutcp, fmt, va);
 171     logPutcp(&bufPtr, '\n');
 172     logPutcp(&bufPtr, 0);
 173     charCount += 2;
 174     va_end(va);
 175
 176     logPrint(buf, charCount);
 177 }
 178
 179 void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t size)
 180 {
 181     // Print lines of up to maxBytes bytes. We need 5 characters per byte
 182     // 0xAB[space|\n]
 183     const size_t charsPerByte = 5;
 184     const size_t maxBytes = 8;
 185     char buf[LOG_PREFIX_FORMATTED_SIZE + charsPerByte * maxBytes + 1]; // +1 for the null terminator
 186     size_t bufPos = LOG_PREFIX_FORMATTED_SIZE;
 187     const uint8_t *inputPtr = buffer;
 188
 189     if (!logIsEnabled(topic, level)) {
 190         return;
 191     }
 192
 193     logFormatPrefix(buf, millis());
 194
 195     for (size_t ii = 0; ii < size; ii++) {
 196         tfp_sprintf(buf + bufPos, "0x%02x ", inputPtr[ii]);
 197         bufPos += charsPerByte;
 198         if (bufPos == sizeof(buf)-1) {
 199             buf[bufPos-1] = '\n';
 200             buf[bufPos] = '\0';
 201             logPrint(buf, bufPos + 1);
 202             bufPos = LOG_PREFIX_FORMATTED_SIZE;
 203         }
 204     }
 205
 206     if (bufPos > LOG_PREFIX_FORMATTED_SIZE) {
 207         buf[bufPos-1] = '\n';
 208         buf[bufPos] = '\0';
 209         logPrint(buf, bufPos + 1);
 210     }
 211 }
 212
 213 #endif