err_t sys_mbox_trypost(sys_mbox_t *mbox, void *msg) {
if (chMBPost(*mbox, (msg_t)msg, TIME_IMMEDIATE) == MSG_TIMEOUT) {
SYS_STATS_INC(mbox.err);
return ERR_MEM;
}
return ERR_OK;
}
// ================================ Inner use ==================================
void Sound_t::AddCmd(uint8_t AAddr, uint16_t AData) {
VsCmd_t FCmd;
FCmd.OpCode = VS_WRITE_OPCODE;
FCmd.Address = AAddr;
FCmd.Data = __REV16(AData);
// Add cmd to queue
chMBPost(&CmdBox, FCmd.Msg, TIME_INFINITE);
StartTransmissionIfNotBusy();
}
void gdispDrawLine(coord_t x0, coord_t y0, coord_t x1, coord_t y1, color_t color) {
gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_DRAWLINE);
p->drawline.x0 = x0;
p->drawline.y0 = y0;
p->drawline.x1 = x1;
p->drawline.y1 = y1;
p->drawline.color = color;
chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
}
void gdispFillArea(coord_t x, coord_t y, coord_t cx, coord_t cy, color_t color) {
gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_FILLAREA);
p->fillarea.x = x;
p->fillarea.y = y;
p->fillarea.cx = cx;
p->fillarea.cy = cy;
p->fillarea.color = color;
chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
}
void gdispFillEllipse(coord_t x, coord_t y, coord_t a, coord_t b, color_t color) {
gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_FILLELLIPSE);
p->fillellipse.x = x;
p->fillellipse.y = y;
p->fillellipse.a = a;
p->fillellipse.b = b;
p->fillellipse.color = color;
chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
}
void gdispDrawChar(coord_t x, coord_t y, char c, font_t font, color_t color) {
gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_DRAWCHAR);
p->drawchar.x = x;
p->drawchar.y = y;
p->drawchar.c = c;
p->drawchar.font = font;
p->drawchar.color = color;
chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
}
/*
* Test worker threads.
*/
static THD_FUNCTION(irq_storm_thread, arg) {
static volatile unsigned x = 0;
static unsigned cnt = 0;
unsigned me = (unsigned)arg;
unsigned target;
unsigned r;
msg_t msg;
chRegSetThreadName("irq_storm");
/* Thread loop, until terminated.*/
while (chThdShouldTerminateX() == false) {
/* Waiting for a message.*/
chMBFetch(&mb[me], &msg, TIME_INFINITE);
#if IRQ_STORM_CFG_RANDOMIZE != FALSE
/* Pseudo-random delay.*/
{
chSysLock();
r = rand() & 15;
chSysUnlock();
while (r--)
x++;
}
#else /* IRQ_STORM_CFG_RANDOMIZE == FALSE */
/* Fixed delay.*/
{
r = me >> 4;
while (r--)
x++;
}
#endif /* IRQ_STORM_CFG_RANDOMIZE == FALSE */
/* Deciding in which direction to re-send the message.*/
if (msg == MSG_SEND_LEFT)
target = me - 1;
else
target = me + 1;
if (target < IRQ_STORM_CFG_NUM_THREADS) {
/* If this thread is not at the end of a chain re-sending the message,
note this check works because the variable target is unsigned.*/
msg = chMBPost(&mb[target], msg, TIME_IMMEDIATE);
if (msg != MSG_OK)
saturated = TRUE;
}
else {
/* Provides a visual feedback about the system.*/
if (++cnt >= 500) {
cnt = 0;
palTogglePad(config->port, config->pad);
}
}
}
}
/*
* log_event
*
* This is how you send things to the logger so it can log them.
* The id parameter must point to an array of at least SDC_MSG_ID_BYTES chars.
* The data_length parameter must accurately represent the length of the array
* pointed to by the data parameter, and this length must be no longer than
* SDC_MSG_MAX_PAYLOAD_BYTES. The returned boolean indicates whether the event
* was succesfully posted (true) or failed because the buffer of events to be
* logged was full (false).
*/
bool log_event(const char* id, const uint8_t* data, uint16_t data_length) {
msg_t status;
GENERIC_message* msg = make_msg(id, data, data_length);
if (msg == NULL) return false;
status = chMBPost(&event_mail, (msg_t) msg, TIME_IMMEDIATE);
return status == RDY_OK;
}
void gdispFillArc(coord_t x, coord_t y, coord_t radius, coord_t start, coord_t end, color_t color) {
gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_FILLARC);
p->fillarc.x = x;
p->fillarc.y = y;
p->fillarc.radius = radius;
p->fillarc.start = start;
p->fillarc.end = end;
p->fillarc.color = color;
chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
}
void gdispFillChar(coord_t x, coord_t y, char c, font_t font, color_t color, color_t bgcolor) {
gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_FILLCHAR);
p->fillchar.x = x;
p->fillchar.y = y;
p->fillchar.c = c;
p->fillchar.font = font;
p->fillchar.color = color;
p->fillchar.bgcolor = bgcolor;
chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
}
void gdispVerticalScroll(coord_t x, coord_t y, coord_t cx, coord_t cy, int lines, color_t bgcolor) {
gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_VERTICALSCROLL);
p->verticalscroll.x = x;
p->verticalscroll.y = y;
p->verticalscroll.cx = cx;
p->verticalscroll.cy = cy;
p->verticalscroll.lines = lines;
p->verticalscroll.bgcolor = bgcolor;
chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
}
void rfm69_log_s64(uint8_t channel, int64_t data)
{
char *msg;
msg = (void*)chPoolAlloc(&rfm69_mp);
msg[4] = (char)(1 | conf.location << 4);
msg[5] = (char)channel;
memcpy(msg, (void*)&halGetCounterValue(), 4);
memcpy(&msg[8], &data, 8);
chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE);
}
void rfm69_log_c(uint8_t channel, const char* data)
{
volatile char *msg;
msg = (char*)chPoolAlloc(&rfm69_mp);
msg[4] = (char)(0 | conf.location << 4);
msg[5] = (char)channel;
memcpy((void*)msg, (void*)&halGetCounterValue(), 4);
memcpy((void*)&msg[8], data, 8);
chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE);
}
static msg_t WorkerThread(void *arg) {
static volatile unsigned x = 0;
static unsigned cnt = 0;
unsigned me = (unsigned)arg;
unsigned target;
unsigned r;
msg_t msg;
chRegSetThreadName("worker");
/* Work loop.*/
while (TRUE) {
/* Waiting for a message.*/
chMBFetch(&mb[me], &msg, TIME_INFINITE);
#if RANDOMIZE
/* Pseudo-random delay.*/
{
chSysLock();
r = rand() & 15;
chSysUnlock();
while (r--)
x++;
}
#else
/* Fixed delay.*/
{
r = me >> 4;
while (r--)
x++;
}
#endif
/* Deciding in which direction to re-send the message.*/
if (msg == MSG_SEND_LEFT)
target = me - 1;
else
target = me + 1;
if (target < NUM_THREADS) {
/* If this thread is not at the end of a chain re-sending the message,
note this check works because the variable target is unsigned.*/
msg = chMBPost(&mb[target], msg, TIME_IMMEDIATE);
if (msg != RDY_OK)
saturated = TRUE;
}
else {
/* Provides a visual feedback about the system.*/
if (++cnt >= 500) {
cnt = 0;
palTogglePad(GPIO0, GPIO0_LED2);
}
}
}
}
void rfm69_log_f(uint8_t channel, float data_a, float data_b)
{
char *msg;
msg = (void*)chPoolAlloc(&rfm69_mp);
msg[4] = (char)(9 | conf.location << 4);
msg[5] = (char)channel;
memcpy(msg, (void*)&halGetCounterValue(), 4);
memcpy(&msg[8], &data_a, 4);
memcpy(&msg[12], &data_b, 4);
chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE);
}
// add a next setpoint for the stepper to go to, will wait if queue is full
void setpointAdd (const Setpoint& s) {
// TODO: silly approach, should use first/last indices into circular buffer
for (int i = 0; i <= SETPOINT_QUEUE_SIZE; ++i)
if (!isInUse(i)) {
setInUse(i);
setpoint.setpoints[i] = s;
chMBPost(&setpoint.mailbox, i, TIME_INFINITE);
return;
}
// never reached
}
void gdispBlitAreaEx(coord_t x, coord_t y, coord_t cx, coord_t cy, coord_t srcx, coord_t srcy, coord_t srccx, const pixel_t *buffer) {
gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_BLITAREA);
p->blitarea.x = x;
p->blitarea.y = y;
p->blitarea.cx = cx;
p->blitarea.cy = cy;
p->blitarea.srcx = srcx;
p->blitarea.srcy = srcy;
p->blitarea.srccx = srccx;
p->blitarea.buffer = buffer;
chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
}
static msg_t tUsbRx(void *arg) {
(void)arg;
chRegSetThreadName("usbRx");
enum {UsbRxComleteID = 0, UsbResetID = 1, UsbConfiguredID = 2};
usbPacket *usbBufp;
EventListener elUsbRxComplete;
EventListener elUsbReset;
EventListener elUsbConfigured;
eventmask_t activeEvents;
chEvtRegister(&esUsbRxComplete, &elUsbRxComplete, UsbRxComleteID);
chEvtRegister(&esUsbReset, &elUsbReset, UsbResetID);
chEvtRegister(&esUsbConfigured, &elUsbConfigured, UsbConfiguredID);
// Wait for the USB system to be configured.
chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
chEvtGetAndClearEvents(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));
while (TRUE) {
// Allocate buffer space for reception of package in the sysctrl mempool
usbBufp = usbAllocMailboxBuffer();
// Prepare receive operation and initiate the usb system to listen
usbPrepareReceive(usbp, EP_OUT, usbBufp->packet, 64);
chSysLock();
usbStartReceiveI(usbp, EP_OUT);
chSysUnlock();
// Wait for events from the USB system
activeEvents = chEvtWaitAny(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));
if (activeEvents == EVENT_MASK(UsbResetID)) {
// If the system was reset, clean up and wait for new configure.
usbFreeMailboxBuffer (usbBufp);
chEvtWaitOne(EVENT_MASK(UsbConfiguredID));
chEvtGetAndClearEvents(EVENT_MASK(UsbRxComleteID) | EVENT_MASK(UsbResetID));
}
else {
// Post pagckage to sysctrl if receive was successful
usbBufp->size = ep2outstate.rxcnt;
chMBPost (&usbRXMailbox, (msg_t)usbBufp, TIME_INFINITE);
}
}
return 0;
}
void rfm69_log_u16(uint8_t channel, uint16_t data_a, uint16_t data_b,
uint16_t data_c, uint16_t data_d)
{
char *msg;
msg = (void*)chPoolAlloc(&rfm69_mp);
msg[4] = (char)(6 | conf.location << 4);
msg[5] = (char)channel;
memcpy(msg, (void*)&halGetCounterValue(), 4);
memcpy(&msg[8], &data_a, 2);
memcpy(&msg[10], &data_b, 2);
memcpy(&msg[12], &data_c, 2);
memcpy(&msg[14], &data_d, 2);
chMBPost(&rfm69_mb, (msg_t)msg, TIME_IMMEDIATE);
}
// Loops constantly in the background.
void matrix_scan_user(void) {
uint8_t page;
uint8_t led_pin_byte;
msg_t msg;
if (backlight_status_global == 0) {//backlight is off, skip the rest
return;
}
if (led_layer_state != layer_state && led_mode_global != GAME && led_mode_global != ALL) {
//check mode
//Turn on layer indicator or page depending on mode
switch(led_mode_global) {
case MODE_FLASH: //flash preset page leds then single indicator
page = biton32(layer_state) > max_pages ? 7 : biton32(layer_state);
msg=(page << 8) | DISPLAY_PAGE;
chMBPost(&led_mailbox, msg, TIME_IMMEDIATE);
chThdSleepMilliseconds(500);
//flow to display single layer leds
case MODE_SINGLE: //light layer indicators for all active layers
led_pin_byte = layer_state & 0xFF;
msg=(7 << 8) | DISPLAY_PAGE;
chMBPost(&led_mailbox, msg, TIME_IMMEDIATE);
msg=(1 << 16) | (led_pin_byte << 8) | SET_FULL_ROW;
chMBPost(&led_mailbox, msg, TIME_IMMEDIATE);
break;
case MODE_PAGE: //display pre-defined led page
page = biton32(layer_state) > max_pages ? 7 : biton32(layer_state);
msg=(page << 8) | DISPLAY_PAGE;
chMBPost(&led_mailbox, msg, TIME_IMMEDIATE);
break;
}
led_layer_state = layer_state;
}
}
void logMsg(char *str)
{
if (logenabled)
{
msg_t m;
m = (msg_t) chPoolAlloc(&logMP);
if ((void *) m != NULL )
{
strncpy((char *) m, str, LOG_MSG_MAX_LENGTH);
((char *) m)[LOG_MSG_MAX_LENGTH - 1] = '\0';
chMBPost(&logMB, m, TIME_IMMEDIATE );
}
}
}
/**
* @brief Put one 'FrameStruct' type pointer into the mailbox
* @details This function is one of the APIs between the NetworkLayer and
* the DataLinkLayer. When a packet need to be sent the NWL will
* call this function individually.
*
* @param[in] driver DataLinkLayer driver structure
* @param[in] frame The frame which need to be put into the mailbox
*
*/
msg_t DLLPutFrameInQueue(DLLDriver *dllp, FrameStruct *Frame){
void *pbuf;
msg_t ReturnValue = chMBFetch(&dllp->DLLBuffers.DLLFreeOutputBuffer, (msg_t *)&pbuf, TIME_INFINITE);
if (ReturnValue == MSG_OK) {
FrameStruct *Temp = pbuf;
int i;
for(i = 0; i < FRAME_SIZE_BYTE; i++)
((char *)Temp)[i] = ((char *)Frame)[i];
Temp->CrcHex = CreateCRC(Temp);
(void)chMBPost(&dllp->DLLBuffers.DLLFilledOutputBuffer, (msg_t)pbuf, TIME_INFINITE);
}
return ReturnValue;
}
/*
* Post a mailbox message to main thread. Use helper functions to
* create the message.
*
*/
void sc_event_msg_post(msg_t msg, SC_EVENT_MSG_POST_FROM from)
{
msg_t ret;
if (from == SC_EVENT_MSG_POST_FROM_ISR) {
chSysLockFromIsr();
ret = chMBPostI(&event_mb, msg);
chDbgAssert(ret == RDY_OK, "chMBPostI failed", "#1");
chSysUnlockFromIsr();
} else {
ret = chMBPost(&event_mb, msg, TIME_IMMEDIATE);
chDbgAssert(ret == RDY_OK, "chMBPost failed", "#1");
}
}
/**
* @brief Put a message in the queue.
*/
osStatus osMessagePut(osMessageQId queue_id,
uint32_t info,
uint32_t millisec) {
msg_t msg;
if (port_is_isr_context()) {
/* Waiting makes no sense in ISRs so any value except "immediate"
makes no sense.*/
if (millisec != 0)
return osErrorValue;
chSysLockFromISR();
msg = chMBPostI((mailbox_t *)queue_id, (msg_t)info);
chSysUnlockFromISR();
}
else
msg = chMBPost((mailbox_t *)queue_id, (msg_t)info, (systime_t)millisec);
return msg == MSG_OK ? osOK : osEventTimeout;
}
static THD_FUNCTION (rxListen, arg)
{
(void)(arg);
uint8_t buffer;
while(!0)
{
/* Read arg3 bytes from device pointed by arg1(SD3 for this example)
* and write data to arg2(buffer for this example). Type of buffer
* should be (uint8_t *). This function blocks this thread until
* desired number of bytes have read*/
//sdRead(&SD3, &header, 9);
sdRead(&SD3, &buffer, 1);
/* Post contents of arg2 to mailbox pointed by arg1, wait to post maximum arg3 mSeconds
* arg3 may be TIME_IMMEDIATE(post if you can) or TIME_INFINITE(wait until post).
* If mailbox object is full, function waits for a free slot for arg3 mSeconds */
chMBPost(&serialMbox, buffer, TIME_INFINITE);
chThdSleepMilliseconds(10);
}
}
/**
* @brief Continuous serial sending thread.
* @details The SDSending thread responsible for the continuous frame sending
* via serial. It receives the frames from the application through a
* mailbox.
*/
static THD_FUNCTION(SDSending, arg) {
chRegSetThreadName("Sending Thread");
DLLDriver *dllp = arg;
void *pbuf;
FrameStruct *Temp;
while(true)
{
dllp->DLLStats.FreeFilledBuffer = chMBGetFreeCountI(&dllp->DLLBuffers.DLLFilledOutputBuffer);
dllp->DLLStats.FreeFreeBuffer = chMBGetFreeCountI(&dllp->DLLBuffers.DLLFreeOutputBuffer);
msg_t msg = chMBFetch(&dllp->DLLBuffers.DLLFilledOutputBuffer, (msg_t *)&pbuf, TIME_INFINITE);
if(msg == MSG_OK)
{
Temp = pbuf;
if(DLLSendSingleFrameSerial(dllp, Temp))
dllp->DLLStats.SentFrames++;
else
dllp->DLLStats.LostFrames++;
(void)chMBPost(&dllp->DLLBuffers.DLLFreeOutputBuffer, (msg_t)pbuf, TIME_INFINITE);
}
}
}
/**
* @brief Initialize the Data Link Layer object.
* @details The function starts the DataLinkLayer serial driver
* - Check the actual state of the driver
* - Configure and start the sdSerial driver
* - Init the mutex variable used by the 'DLLSendSingleFrameSerial' function
* - Init the mailboxes which are work like a buffer
* - Creates a SyncFrame
* - Starts the 'SDReceiving' and 'SDSending' threads which are provide
* the whole DLL functionality
* - Set the DLL state to ACTIVE
*
* @param[in] dllp DataLinkLayer driver structure
* @param[in] config The config contains the speed and the ID of the serial driver
*/
void DLLStart(DLLDriver *dllp, DLLSerialConfig *config){
osalDbgCheck((dllp != NULL) && (config != NULL));
osalDbgAssert((dllp->state == DLL_UNINIT) || (dllp->state == DLL_ACTIVE),
"DLLInit(), invalid state");
dllp->config = config;
SerialDCfg.speed = dllp->config->baudrate; //Set the data rate to the given rate
sdStart(dllp->config->SDriver, &SerialDCfg); //Start the serial driver for the ESP8266
chMtxObjectInit(&dllp->DLLSerialSendMutex);
chMBObjectInit(&dllp->DLLBuffers.DLLFilledOutputBuffer,
dllp->DLLBuffers.DLLFilledOutputBufferQueue, OUTPUT_FRAME_BUFFER);
chMBObjectInit(&dllp->DLLBuffers.DLLFreeOutputBuffer,
dllp->DLLBuffers.DLLFreeOutputBufferQueue, OUTPUT_FRAME_BUFFER);
int i;
for (i = 0; i < OUTPUT_FRAME_BUFFER; i++)
(void)chMBPost(&dllp->DLLBuffers.DLLFreeOutputBuffer,
(msg_t)&dllp->DLLBuffers.DLLOutputBuffer[i], TIME_INFINITE);
DLLCreateSyncFrame(dllp);
dllp->SendingThread = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(128), NORMALPRIO+1, SDSending, (void *)dllp);
if (dllp->SendingThread == NULL)
chSysHalt("DualFramework: Starting 'SendingThread' failed - out of memory");
dllp->ReceivingThread = chThdCreateFromHeap(NULL, THD_WORKING_AREA_SIZE(128), NORMALPRIO+1, SDReceiving, (void *)dllp);
if (dllp->ReceivingThread == NULL)
chSysHalt("DualFramework: Starting 'ReceivingThread' failed - out of memory");
dllp->state = DLL_ACTIVE;
}
void gdispClear(color_t color) {
gdisp_lld_msg_t *p = gdispAllocMsg(GDISP_LLD_MSG_CLEAR);
p->clear.color = color;
chMBPost(&gdispMailbox, (msg_t)p, TIME_INFINITE);
}
void sys_mbox_post(sys_mbox_t *mbox, void *msg) {
chMBPost(*mbox, (msg_t)msg, TIME_INFINITE);
}
msg_t Mailbox::post(msg_t msg, systime_t time) {
return chMBPost(&mb, msg, time);
}
