mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-10 09:58:28 -04:00
AP_Common: replace strlcat_P() with a C implementation
the one in the Arduino libc was giving us bogus results on one machine, and is suspect. We couldn't spot what is wrong in the assembler, but replacing the implementation with a C one fixes the bug, so we replaced it
This commit is contained in:
parent
a048709828
commit
92e0e929a4
@ -105,9 +105,34 @@ static inline int strcmp_P(const char *str1, const prog_char_t *pstr)
|
||||
return strcmp_P(str1, (const prog_char *)pstr);
|
||||
}
|
||||
|
||||
static inline size_t strlcat_P(char *buffer, const prog_char_t *pstr, size_t buffer_size)
|
||||
static inline size_t strlen_P(const prog_char_t *pstr)
|
||||
{
|
||||
return strlcat_P(buffer, (const prog_char *)pstr, buffer_size);
|
||||
return strlen_P((const prog_char *)pstr);
|
||||
}
|
||||
|
||||
// strlcat_P() in AVR libc seems to be broken
|
||||
static inline size_t strlcat_P(char *d, const prog_char_t *s, size_t bufsize)
|
||||
{
|
||||
size_t len1 = strlen(d);
|
||||
size_t len2 = strlen_P(s);
|
||||
size_t ret = len1 + len2;
|
||||
|
||||
if (len1+len2 >= bufsize) {
|
||||
if (bufsize < (len1+1)) {
|
||||
return ret;
|
||||
}
|
||||
len2 = bufsize - (len1+1);
|
||||
}
|
||||
if (len2 > 0) {
|
||||
memcpy_P(d+len1, s, len2);
|
||||
d[len1+len2] = 0;
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
static inline char *strncpy_P(char *buffer, const prog_char_t *pstr, size_t buffer_size)
|
||||
{
|
||||
return strncpy_P(buffer, (const prog_char *)pstr, buffer_size);
|
||||
}
|
||||
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user