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