static void dyn1_execute(void) {
size_t n, sz;
void *p1;
tprio_t prio = chThdGetPriority();
(void)chHeapStatus(&heap1, &sz);
/* Starting threads from the heap. */
threads[0] = chThdCreateFromHeap(&heap1, THD_WA_SIZE(THREADS_STACK_SIZE),
prio-1, thread, "A");
threads[1] = chThdCreateFromHeap(&heap1, THD_WA_SIZE(THREADS_STACK_SIZE),
prio-2, thread, "B");
/* Allocating the whole heap in order to make the thread creation fail.*/
(void)chHeapStatus(&heap1, &n);
p1 = chHeapAlloc(&heap1, n);
threads[2] = chThdCreateFromHeap(&heap1, THD_WA_SIZE(THREADS_STACK_SIZE),
prio-3, thread, "C");
chHeapFree(p1);
test_assert(1, (threads[0] != NULL) &&
(threads[1] != NULL) &&
(threads[2] == NULL) &&
(threads[3] == NULL) &&
(threads[4] == NULL),
"thread creation failed");
/* Claiming the memory from terminated threads. */
test_wait_threads();
test_assert_sequence(2, "AB");
/* Heap status checked again.*/
test_assert(3, chHeapStatus(&heap1, &n) == 1, "heap fragmented");
test_assert(4, n == sz, "heap size changed");
}
/**
* @brief Releases a reference to a thread object.
* @details If the references counter reaches zero <b>and</b> the thread
* is in the @p THD_STATE_FINAL state then the thread's memory is
* returned to the proper allocator.
* @pre The configuration option @p CH_USE_DYNAMIC must be enabled in order
* to use this function.
* @note Static threads are not affected.
*
* @param[in] tp pointer to the thread
*
* @api
*/
void chThdRelease(Thread *tp) {
trefs_t refs;
chSysLock();
chDbgAssert(tp->p_refs > 0, "chThdRelease(), #1", "not referenced");
refs = --tp->p_refs;
chSysUnlock();
/* If the references counter reaches zero and the thread is in its
terminated state then the memory can be returned to the proper
allocator. Of course static threads are not affected.*/
if ((refs == 0) && (tp->p_state == THD_STATE_FINAL)) {
switch (tp->p_flags & THD_MEM_MODE_MASK) {
#if CH_USE_HEAP
case THD_MEM_MODE_HEAP:
#if CH_USE_REGISTRY
REG_REMOVE(tp);
#endif
chHeapFree(tp);
break;
#endif
#if CH_USE_MEMPOOLS
case THD_MEM_MODE_MEMPOOL:
#if CH_USE_REGISTRY
REG_REMOVE(tp);
#endif
chPoolFree(tp->p_mpool, tp);
break;
#endif
}
}
}
/**
* @brief Free the specific memory block
*/
void mangoPort_free(void* ptr){
#ifdef MANGO_OS_ENV__UNIX
free(ptr);
#endif
#ifdef MANGO_OS_ENV__CHIBIOS
chHeapFree(ptr);
#endif
}
void
vexAudioDeinitChipToneSong()
{
// already have an array ?
if(VSL_ctarray != NULL)
{
chHeapFree(VSL_ctarray);
VSL_ctarray = NULL;
ctSize = 0;
}
}
void osFreeMem(void *p)
{
//Make sure the pointer is valid
if(p != NULL)
{
//Debug message
TRACE_DEBUG("Freeing memory at 0x%08" PRIXPTR "\r\n", (uintptr_t) p);
//Free memory block
chHeapFree(p);
}
}
GSourceHandle ginputGetMouse(uint16_t instance) {
#if GINPUT_MOUSE_NEED_CALIBRATION
Calibration *pc;
#endif
// We only support a single mouse instance currently
// Instance 9999 is the same as instance 0 except that it installs
// a special "raw" calibration if there isn't one we can load.
if (instance && instance != 9999)
return 0;
// Do we need to initialise the mouse subsystem?
if (!(MouseConfig.flags & FLG_INIT_DONE)) {
ginput_lld_mouse_init();
#if GINPUT_MOUSE_NEED_CALIBRATION
#if GINPUT_MOUSE_LLD_CALIBRATION_LOADSAVE
if (!MouseConfig.fnloadcal) {
MouseConfig.fnloadcal = ginput_lld_mouse_calibration_load;
MouseConfig.flags &= ~FLG_CAL_FREE;
}
if (!MouseConfig.fnsavecal)
MouseConfig.fnsavecal = ginput_lld_mouse_calibration_save;
#endif
if (MouseConfig.fnloadcal && (pc = (Calibration *)MouseConfig.fnloadcal(instance))) {
MouseConfig.caldata = pc[0];
MouseConfig.flags |= (FLG_CAL_OK|FLG_CAL_SAVED);
if ((MouseConfig.flags & FLG_CAL_FREE))
chHeapFree((void *)pc);
} else if (instance == 9999) {
MouseConfig.caldata.ax = 1;
MouseConfig.caldata.bx = 0;
MouseConfig.caldata.cx = 0;
MouseConfig.caldata.ay = 0;
MouseConfig.caldata.by = 1;
MouseConfig.caldata.cy = 0;
MouseConfig.flags |= (FLG_CAL_OK|FLG_CAL_SAVED);
} else
ginputCalibrateMouse(instance);
#endif
// Get the first reading
MouseConfig.last_buttons = 0;
get_calibrated_reading(&MouseConfig.t);
// Mark init as done and start the Poll timer
MouseConfig.flags |= FLG_INIT_DONE;
gtimerStart(&MouseTimer, MousePoll, 0, TRUE, GINPUT_MOUSE_POLL_PERIOD);
}
// Return our structure as the handle
return (GSourceHandle)&MouseConfig;
}
static void dyn_release_object_heap(dyn_element_t *dep,
dyn_list_t *dlp) {
chDbgCheck(dep != NULL);
chDbgAssert(dep->refs > (ucnt_t)0, "invalid references number");
dep->refs--;
if (dep->refs == (ucnt_t)0) {
dep = dyn_list_unlink(dep, dlp);
chHeapFree((void *)dep);
}
}
void sys_mbox_free(sys_mbox_t *mbox) {
if (chMBGetUsedCountI(*mbox) != 0) {
// If there are messages still present in the mailbox when the mailbox
// is deallocated, it is an indication of a programming error in lwIP
// and the developer should be notified.
SYS_STATS_INC(mbox.err);
chMBReset(*mbox);
}
chHeapFree(*mbox);
*mbox = SYS_MBOX_NULL;
SYS_STATS_DEC(mbox.used);
}
static msg_t sat_thread(void *p)
{
msg_t msg, result;
struct packet_msg *pkt;
(void)p;
while (TRUE) {
msg = fetch_sat();
pkt = (struct packet_msg *) msg;
chprintf((BaseSequentialStream *)&SDDBG, "SAT\r\n");
chHeapFree(pkt);
}
return 0;
}
static void process_packet(struct packet_msg *p)
{
int res;
switch(p->id) {
case 0x20: /* Geodata */
res = post_geo(p);
break;
case 0x21: /* Telemetry */
res = post_tel(p);
break;
case 0x22: /* Sats channels */
res = post_sat(p);
break;
default:
pr_debug("bad %02x\r\n", p->id);
chHeapFree(p);
return;
break;
}
if (res != RDY_OK) {
pr_debug("timeout\r\n");
chHeapFree(p);
}
}
int
vexAudioInitChipToneSong( int len )
{
int i;
vsl_ct *p;
// already have an array ?
if(VSL_ctarray != NULL)
{
if( ctSize < len )
{
chHeapFree(VSL_ctarray);
VSL_ctarray = NULL;
}
}
// allocate some storage for the chiptone array if we need to
if(VSL_ctarray == NULL)
VSL_ctarray = (vsl_ct *)chHeapAlloc( NULL, sizeof(vsl_ct) * (len + 1) );
// clear everything
ctSetPtr = 0;
ctSize = 0;
ctPlayPtr = 0;
ctRepeat = 0;
// clear array
if( VSL_ctarray != NULL )
{
p = VSL_ctarray;
for(i=0;i<len;i++,p++)
{
p->freq = 0;
#ifdef VSL_AMP_PER_TONE
p->amplitude = 0;
#endif
p->timems = 0;
}
ctSize = len;
return(1); // success
}
else
return(0); // error
}
/* CRC */
case 100:
case 101:
case 102:
state++;
break;
case 103:
state = 0;
if(crc) {
printf("b\r\n");
chHeapFree(packet);
} else {
process_packet(packet);
}
packet_count = 0;
break;
}
}
static msg_t geo_thread(void *p)
{
msg_t msg, result;
struct packet_msg *pkt;
(void)p;
while (TRUE) {
msg = fetch_geo();
pkt = (struct packet_msg *) msg;
Tgeos1m_packet_geographic *Pmsg = (Tgeos1m_packet_geographic *)pkt->data;
double time_s_int;
double time_s_fract = modf(Pmsg->time_s, &time_s_int);
time_t tstamp = 0x47798280 + (int)time_s_int;
struct tm *gtim = gmtime(&tstamp);
chprintf((BaseSequentialStream *)&SDDBG, "GEOGRAPH PARSE tstamp = %ld\r\n", tstamp);
geoinfo.dtime.msec = (int)(time_s_fract * 1000);
geoinfo.dtime.sec = gtim->tm_sec;
geoinfo.dtime.min = gtim->tm_min;
geoinfo.dtime.hour = gtim->tm_hour;
geoinfo.dtime.mday = gtim->tm_mday;
geoinfo.dtime.mon = gtim->tm_mon; //(Months *since* january: 0-11)
geoinfo.dtime.year = gtim->tm_year + 1900 - 2000;
geoinfo.lat = Pmsg->lat;
geoinfo.lon = Pmsg->lon;
geoinfo.alt = Pmsg->alt;
geo_motion2D_pol2geo(Pmsg->course, Pmsg->speed, &geoinfo.motion);// ñêîðîñòü, êóðñ
geo_DOP2geo(Pmsg->HDOP, Pmsg->VDOP, Pmsg->TDOP, &geoinfo.prec);
#if 0
DEBUGCRLF(GNSS, DETAILED);
geo_time_result();//geo&time debug prited by this function
dop_result();
#endif
chHeapFree(pkt);
}
return 0;
}
#define GEOS1M_STATUS_ANTV (1 << 22) /* Antenna voltage */
static msg_t tel_thread(void *p)
{
msg_t msg, result;
struct packet_msg *pkt;
uint32_t status;
(void)p;
Tgeos1m_packet_cur_telemetry *Pmsg;
while (TRUE) {
msg = fetch_tel();
pkt = (struct packet_msg *) msg;
chprintf((BaseSequentialStream *)&SDDBG, "TELEMETRY\r\n");
Pmsg = (Tgeos1m_packet_cur_telemetry *)pkt->data;
status = Pmsg->status;
set_antenna_status(!(status & GEOS1M_STATUS_ANTV));
decision_complete();
chHeapFree(pkt);
}
return 0;
}
static int detect_geos(void)
{
int i, j;
uint8_t *gnss_buffer = chHeapAlloc(NULL, 1024);
int detected = 0;
palSetPad(IOPORT1, PIOA_GPS_NRST);
chThdSleepMilliseconds(100);
k2_usart1_geos();
for (i = 0; i < 20; i++) {
int t = sdReadTimeout(&SD1, gnss_buffer, 1024, 250);
if (t > 0) {
for (j = 0; j < t; j++) {
if (packet_header(gnss_buffer[j],
(uint8_t *) "PSGG", 4))
detected = 1;
}
}
}
chHeapFree(gnss_buffer);
return detected;
}
/**
* @brief Releases a reference to a thread object.
* @details If the references counter reaches zero <b>and</b> the thread
* is in the @p CH_STATE_FINAL state then the thread's memory is
* returned to the proper allocator.
* @pre The configuration option @p CH_CFG_USE_DYNAMIC must be enabled in
* order to use this function.
* @note Static threads are not affected.
*
* @param[in] tp pointer to the thread
*
* @api
*/
void chThdRelease(thread_t *tp) {
trefs_t refs;
chSysLock();
chDbgAssert(tp->p_refs > (trefs_t)0, "not referenced");
tp->p_refs--;
refs = tp->p_refs;
/* If the references counter reaches zero and the thread is in its
terminated state then the memory can be returned to the proper
allocator. Of course static threads are not affected.*/
if ((refs == (trefs_t)0) && (tp->p_state == CH_STATE_FINAL)) {
switch (tp->p_flags & CH_FLAG_MODE_MASK) {
#if CH_CFG_USE_HEAP == TRUE
case CH_FLAG_MODE_HEAP:
#if CH_CFG_USE_REGISTRY == TRUE
REG_REMOVE(tp);
#endif
chSysUnlock();
chHeapFree(tp);
return;
#endif
#if CH_CFG_USE_MEMPOOLS == TRUE
case CH_FLAG_MODE_MPOOL:
#if CH_CFG_USE_REGISTRY == TRUE
REG_REMOVE(tp);
#endif
chSysUnlock();
chPoolFree(tp->p_mpool, tp);
return;
#endif
default:
/* Nothing to do for static threads, those are removed from the
registry on exit.*/
break;
}
}
chSysUnlock();
}
static msg_t geo_thread(void *p)
{
msg_t msg, result;
struct packet_msg *pkt;
(void)p;
while (TRUE) {
msg = fetch_geo();
pkt = (struct packet_msg *) msg;
Tgeos1m_packet_geographic *Pmsg = (Tgeos1m_packet_geographic *)pkt->data;
double time_s_int;
double time_s_fract = modf(Pmsg->time_s, &time_s_int);
time_t tstamp = 0x47798280 + (int)time_s_int;
struct tm *gtim = gmtime(&tstamp);
chprintf((BaseSequentialStream *)&SDDBG, "GEOGRAPH PARSE tstamp = %ld\r\n", tstamp);
geoinfo.dtime.msec = (int)(time_s_fract * 1000);
geoinfo.dtime.sec = gtim->tm_sec;
geoinfo.dtime.min = gtim->tm_min;
geoinfo.dtime.hour = gtim->tm_hour;
geoinfo.dtime.mday = gtim->tm_mday;
geoinfo.dtime.mon = gtim->tm_mon; //(Months *since* january: 0-11)
geoinfo.dtime.year = gtim->tm_year + 1900 - 2000;
geoinfo.lat = Pmsg->lat;
geoinfo.lon = Pmsg->lon;
geoinfo.alt = Pmsg->alt;
geo_motion2D_pol2geo(Pmsg->course, Pmsg->speed, &geoinfo.motion);// ñêîðîñòü, êóðñ
geo_DOP2geo(Pmsg->HDOP, Pmsg->VDOP, Pmsg->TDOP, &geoinfo.prec);
#if 0
DEBUGCRLF(GNSS, DETAILED);
geo_time_result();//geo&time debug prited by this function
dop_result();
#endif
chHeapFree(pkt);
}
return 0;
}
/* See documentation in header file. */
int
ini_parse_file(FIL* file, int
(*handler)(void*, const char*, const char*, const char*), void* user)
{
/* Uses a fair bit of stack (use heap instead if you need to) */
#if INI_USE_STACK
char line[INI_MAX_LINE];
#else
char* line;
#endif
char section[MAX_SECTION] = "";
char prev_name[MAX_NAME] = "";
char* start;
char* end;
char* name;
char* value;
int lineno = 0;
int error = 0;
#if !INI_USE_STACK
line = (char*) chHeapAlloc(NULL, INI_MAX_LINE);
if (!line)
{
return -2;
}
#endif
/* Scan through file line by line */
while (f_gets(line, INI_MAX_LINE, file) != NULL)
{
lineno++;
start = line;
#if INI_ALLOW_BOM
if (lineno == 1 && (unsigned char) start[0] == 0xEF
&& (unsigned char) start[1] == 0xBB
&& (unsigned char) start[2] == 0xBF)
{
start += 3;
}
#endif
start = lskip(rstrip(start));
if (*start == ';' || *start == '#')
{
/* Per Python ConfigParser, allow '#' comments at start of line */
}
#if INI_ALLOW_MULTILINE
else if (*prev_name && *start && start > line)
{
/* Non-black line with leading whitespace, treat as continuation
of previous name's value (as per Python ConfigParser). */
if (!handler(user, section, prev_name, start) && !error)
error = lineno;
}
#endif
else if (*start == '[')
{
/* A "[section]" line */
end = find_char_or_comment(start + 1, ']');
if (*end == ']')
{
*end = '\0';
strncpy0(section, start + 1, sizeof(section));
*prev_name = '\0';
}
else if (!error)
{
/* No ']' found on section line */
error = lineno;
}
}
else if (*start && *start != ';')
{
/* Not a comment, must be a name[=:]value pair */
end = find_char_or_comment(start, '=');
if (*end != '=')
{
end = find_char_or_comment(start, ':');
}
if (*end == '=' || *end == ':')
{
*end = '\0';
name = rstrip(start);
value = lskip(end + 1);
end = find_char_or_comment(value, '\0');
if (*end == ';')
*end = '\0';
rstrip(value);
/* Valid name[=:]value pair found, call handler */
strncpy0(prev_name, name, sizeof(prev_name));
if (!handler(user, section, name, value) && !error)
error = lineno;
}
else if (!error)
{
/* No '=' or ':' found on name[=:]value line */
error = lineno;
}
}
}
#if !INI_USE_STACK
chHeapFree(line);
#endif
return error;
}
static void heap1_execute(void) {
void *p1, *p2, *p3;
size_t n, sz;
/* Unrelated, for coverage only.*/
(void)chCoreStatus();
/*
* Test on the default heap in order to cover the core allocator at
* least one time.
*/
(void)chHeapStatus(NULL, &sz);
p1 = chHeapAlloc(NULL, SIZE);
test_assert(1, p1 != NULL, "allocation failed");
chHeapFree(p1);
p1 = chHeapAlloc(NULL, (size_t)-256);
test_assert(2, p1 == NULL, "allocation not failed");
/* Initial local heap state.*/
(void)chHeapStatus(&test_heap, &sz);
/* Same order.*/
p1 = chHeapAlloc(&test_heap, SIZE);
p2 = chHeapAlloc(&test_heap, SIZE);
p3 = chHeapAlloc(&test_heap, SIZE);
chHeapFree(p1); /* Does not merge.*/
chHeapFree(p2); /* Merges backward.*/
chHeapFree(p3); /* Merges both sides.*/
test_assert(3, chHeapStatus(&test_heap, &n) == 1, "heap fragmented");
/* Reverse order.*/
p1 = chHeapAlloc(&test_heap, SIZE);
p2 = chHeapAlloc(&test_heap, SIZE);
p3 = chHeapAlloc(&test_heap, SIZE);
chHeapFree(p3); /* Merges forward.*/
chHeapFree(p2); /* Merges forward.*/
chHeapFree(p1); /* Merges forward.*/
test_assert(4, chHeapStatus(&test_heap, &n) == 1, "heap fragmented");
/* Small fragments handling.*/
p1 = chHeapAlloc(&test_heap, SIZE + 1);
p2 = chHeapAlloc(&test_heap, SIZE);
chHeapFree(p1);
test_assert(5, chHeapStatus(&test_heap, &n) == 2, "invalid state");
p1 = chHeapAlloc(&test_heap, SIZE);
/* Note, the first situation happens when the alignment size is smaller
than the header size, the second in the other cases.*/
test_assert(6, (chHeapStatus(&test_heap, &n) == 1) ||
(chHeapStatus(&test_heap, &n) == 2), "heap fragmented");
chHeapFree(p2);
chHeapFree(p1);
test_assert(7, chHeapStatus(&test_heap, &n) == 1, "heap fragmented");
/* Skip fragment handling.*/
p1 = chHeapAlloc(&test_heap, SIZE);
p2 = chHeapAlloc(&test_heap, SIZE);
chHeapFree(p1);
test_assert(8, chHeapStatus(&test_heap, &n) == 2, "invalid state");
p1 = chHeapAlloc(&test_heap, SIZE * 2); /* Skips first fragment.*/
chHeapFree(p1);
chHeapFree(p2);
test_assert(9, chHeapStatus(&test_heap, &n) == 1, "heap fragmented");
/* Allocate all handling.*/
(void)chHeapStatus(&test_heap, &n);
p1 = chHeapAlloc(&test_heap, n);
test_assert(10, chHeapStatus(&test_heap, &n) == 0, "not empty");
chHeapFree(p1);
test_assert(11, chHeapStatus(&test_heap, &n) == 1, "heap fragmented");
test_assert(12, n == sz, "size changed");
}
void sys_sem_free(sys_sem_t *sem) {
chHeapFree(*sem);
*sem = SYS_SEM_NULL;
SYS_STATS_DEC(sem.used);
}
void operator delete[](void* p) noexcept {
chHeapFree(p);
}
/*------------------------------------------------------------------------*/
void ff_memfree(void *mblock) {
chHeapFree(mblock);
}
static msg_t GPSThread(void *arg) {
(void)arg;
chRegSetThreadName("gps_thread");
if (gps_data != NULL) {
chHeapFree(gps_data);
}
gps_data = chHeapAlloc(NULL, GPS_CMD_BUF);
size_t gps_bytes_read;
uint16_t i;
if (gps_data == NULL) {
while (TRUE) {
palTogglePad(GPIO_LED_1_PORT, GPIO_LED_1_PIN);
chThdSleepMilliseconds(50);
}
}
sdStart(&GPS_SERIAL, &SD3_Config);
palSetPadMode(GPS_USART_PORT, GPS_USART_TX_PIN, PAL_MODE_ALTERNATE(7));
palSetPadMode(GPS_USART_PORT, GPS_USART_RX_PIN, PAL_MODE_ALTERNATE(7));
gps_reset();
while (TRUE) {
size_t readed_msg_len = 0;
uint8_t res = gps_read_msg(&readed_msg_len);
if (res == E_OK) {
if (check_checksum() != E_OK) {
sdWrite(&SD1, "GPS CHK ERR\r\n", 13);
} else {
if (gps_message_type() == GPS_MESSAGE_GPRMC) {
gps_rmc_state_t state;
//sdWrite(&SD1, gps_data, readed_msg_len);
if (parse_gps_rmc(&state) == E_OK) {
sdWrite(&SD1, "GPS PARSE OK\r\n", 14);
}
} else if (gps_message_type() == GPS_MESSAGE_UNKNOWN) {
sdWrite(&SD1, "GPS MSG UNKNOWN\r\n", 17);
}
}
} else {
gps_data[0] = '0' + res;
sdWrite(&SD1, "GPS ERROR:", 10);
sdWrite(&SD1, gps_data, 1);
gps_reset();
}
sdWrite(&SD1, "\r\n", 2);
//sdWrite(&SD1, "GPS: ", 5);
//while ((gps_bytes_read = sdReadTimeout(&GPS_SERIAL, gps_data, GPS_CMD_BUF, 100)) > 0)
// sdWrite(&SD1, gps_data, gps_bytes_read);
//sdWrite(&SD1, "|||\r\n", 5);
//chThdSleepMilliseconds(500);
}
chHeapFree(gps_data);
}
void operator delete[](void *p) {
chHeapFree(p);
}
void packet_detector_geos(int c)
{
static int state = 0;
static int packetnum = 0;
static int packetsize = 0;
static int packet_count = 0;
int crc;
static struct packet_msg *packet = NULL;
if (state != 0)
crc = geos_crc(c, 0);
switch(state) {
case 0:
if (packet_header(c, (uint8_t *)"PSGG", 4)) {
state = 1;
geos_crc('P', 1);
geos_crc('S', 0);
geos_crc('G', 0);
geos_crc('G', 0);
}
break;
case 1:
packetnum = c;
state = 2;
break;
case 2:
packetnum |= (c << 8);
state = 3;
break;
case 3:
packetsize = c;
state = 4;
break;
case 4:
packetsize |= (c << 8);
state = 5;
packet = chHeapAlloc(NULL,
sizeof(struct packet_msg) + packetsize * 4);
if (!packet) {
printf("Drop\r\n");
state = 0;
break;
}
packet->len = packetsize *4;
packet->id = packetnum;
break;
case 5:
case 6:
case 7:
case 8:
if (packet) {
if (packet->len > packet_count) {
packet->data[packet_count] = c;
packet_count++;
}
}
if (state == 8) {
packetsize--;
if(!packetsize)
state = 100;
else
state = 5;
} else
state++;
break;
/* CRC */
case 100:
case 101:
case 102:
state++;
break;
case 103:
state = 0;
if(crc) {
printf("b\r\n");
chHeapFree(packet);
} else {
process_packet(packet);
}
packet_count = 0;
break;
}
}
