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) || defined(SITL_BUILD)
  24 #include <stdio.h>
  25 #endif
  26
  27 #include "build/version.h"
  28 #include "build/debug.h"
  29
  30 #include "drivers/serial.h"
  31 #include "drivers/time.h"
  32
  33 #include "common/log.h"
  34 #include "common/printf.h"
  35 #include "common/utils.h"
  36
  37 #include "config/feature.h"
  38 #include "config/parameter_group_ids.h"
  39
  40 #include "io/serial.h"
  41
  42 #include "fc/config.h"
  43 #include "fc/settings.h"
  44
  45 #include "msp/msp.h"
  46 #include "msp/msp_serial.h"
  47 #include "msp/msp_protocol.h"
  48
  49 #if defined(USE_LOG)
  50
  51 #define LOG_PREFIX                  "[%6d.%03d] "
  52 #define LOG_PREFIX_FORMATTED_SIZE   13
  53
  54 static serialPort_t * logPort = NULL;
  55 static mspPort_t * mspLogPort = NULL;
  56
  57 PG_REGISTER(logConfig_t, logConfig, PG_LOG_CONFIG, 0);
  58
  59 PG_RESET_TEMPLATE(logConfig_t, logConfig,
  60     .level = SETTING_LOG_LEVEL_DEFAULT,
  61     .topics = SETTING_LOG_TOPICS_DEFAULT
  62 );
  63
  64 void logInit(void)
  65 {
  66     const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_LOG);
  67     if (!portConfig) {
  68         return;
  69     }
  70
  71     bool portIsSharedWithMSP = false;
  72
  73     if (determinePortSharing(portConfig, FUNCTION_LOG) == PORTSHARING_SHARED) {
  74         // We support sharing a LOG port only with MSP
  75         if (portConfig->functionMask != (FUNCTION_LOG | FUNCTION_MSP)) {
  76             return;
  77         }
  78         portIsSharedWithMSP = true;
  79     }
  80
  81     // If the port is shared with MSP, reuse the port
  82     if (portIsSharedWithMSP) {
  83         const serialPort_t *logAndMspPort = findSharedSerialPort(FUNCTION_LOG, FUNCTION_MSP);
  84         if (!logAndMspPort) {
  85             return;
  86         }
  87
  88         mspLogPort = mspSerialPortFind(logAndMspPort);
  89         if (!mspLogPort) {
  90             return;
  91         }
  92
  93     } else {
  94         logPort = openSerialPort(portConfig->identifier, FUNCTION_LOG, NULL, NULL, baudRates[BAUD_921600], MODE_TX, SERIAL_NOT_INVERTED);
  95         if (!logPort) {
  96             return;
  97         }
  98     }
  99     // Initialization done
 100     LOG_INFO(SYSTEM, "%s/%s %s %s / %s (%s)",
 101         FC_FIRMWARE_NAME,
 102         targetName,
 103         FC_VERSION_STRING,
 104         buildDate,
 105         buildTime,
 106         shortGitRevision
 107     );
 108 }
 109
 110 static void logPutcp(void *p, char ch)
 111 {
 112     *(*((char **) p))++ = ch;
 113 }
 114
 115 static void logPrint(const char *buf, size_t size)
 116 {
 117 #if defined(SEMIHOSTING)
 118     static bool semihostingInitialized = false;
 119     extern void initialise_monitor_handles(void);
 120
 121     if (!semihostingInitialized) {
 122         initialise_monitor_handles();
 123         semihostingInitialized = true;
 124     }
 125     for (size_t ii = 0; ii < size; ii++) {
 126         fputc(buf[ii], stdout);
 127     }
 128 #endif
 129     SD(printf("%s\n", buf));
 130     if (logPort) {
 131         // Send data via UART (if configured & connected - a safeguard against zombie VCP)
 132         if (serialIsConnected(logPort)) {
 133             serialPrint(logPort, buf);
 134         }
 135     } else if (mspLogPort) {
 136         mspSerialPushPort(MSP_DEBUGMSG, (uint8_t*)buf, size, mspLogPort, MSP_V2_NATIVE);
 137     }
 138 }
 139
 140 static size_t logFormatPrefix(char *buf, const timeMs_t timeMs)
 141 {
 142     // Write timestamp
 143     return tfp_sprintf(buf, LOG_PREFIX, (int)(timeMs / 1000), (int)(timeMs % 1000));
 144 }
 145
 146 static bool logHasOutput(void)
 147 {
 148 #if defined(SEMIHOSTING)
 149     return true;
 150 #else
 151     return logPort || mspLogPort;
 152 #endif
 153 }
 154
 155 static bool logIsEnabled(logTopic_e topic, unsigned level)
 156 {
 157     return logHasOutput() && (level <= logConfig()->level || (logConfig()->topics & (1 << topic)));
 158 }
 159
 160 void _logf(logTopic_e topic, unsigned level, const char *fmt, ...)
 161 {
 162     char buf[128];
 163     char *bufPtr;
 164     int charCount;
 165
 166     STATIC_ASSERT(MSP_PORT_OUTBUF_SIZE >= sizeof(buf), MSP_PORT_OUTBUF_SIZE_not_big_enough_for_log);
 167
 168     if (!logIsEnabled(topic, level)) {
 169         return;
 170     }
 171
 172     charCount = logFormatPrefix(buf, millis());
 173     bufPtr = &buf[charCount];
 174
 175     // Write message
 176     va_list va;
 177     va_start(va, fmt);
 178     charCount += tfp_format(&bufPtr, logPutcp, fmt, va);
 179     logPutcp(&bufPtr, '\n');
 180     logPutcp(&bufPtr, 0);
 181     charCount += 2;
 182     va_end(va);
 183
 184     logPrint(buf, charCount);
 185 }
 186
 187 void _logBufferHex(logTopic_e topic, unsigned level, const void *buffer, size_t size)
 188 {
 189     // Print lines of up to maxBytes bytes. We need 5 characters per byte
 190     // 0xAB[space|\n]
 191     const size_t charsPerByte = 5;
 192     const size_t maxBytes = 8;
 193     char buf[LOG_PREFIX_FORMATTED_SIZE + charsPerByte * maxBytes + 1]; // +1 for the null terminator
 194     size_t bufPos = LOG_PREFIX_FORMATTED_SIZE;
 195     const uint8_t *inputPtr = buffer;
 196
 197     if (!logIsEnabled(topic, level)) {
 198         return;
 199     }
 200
 201     logFormatPrefix(buf, millis());
 202
 203     for (size_t ii = 0; ii < size; ii++) {
 204         tfp_sprintf(buf + bufPos, "0x%02x ", inputPtr[ii]);
 205         bufPos += charsPerByte;
 206         if (bufPos == sizeof(buf)-1) {
 207             buf[bufPos-1] = '\n';
 208             buf[bufPos] = '\0';
 209             logPrint(buf, bufPos + 1);
 210             bufPos = LOG_PREFIX_FORMATTED_SIZE;
 211         }
 212     }
 213
 214     if (bufPos > LOG_PREFIX_FORMATTED_SIZE) {
 215         buf[bufPos-1] = '\n';
 216         buf[bufPos] = '\0';
 217         logPrint(buf, bufPos + 1);
 218     }
 219 }
 220
 221 #endif