346 lines
10 KiB
C
346 lines
10 KiB
C
#include "main.h"
|
|
#include <cstdio>
|
|
#include <stdbool.h>
|
|
#include <stdint.h>
|
|
#include <string.h>
|
|
#include <cassert>
|
|
|
|
uint8_t* lastToken; // Pointer to last token
|
|
uint8_t* lastSentMsgPtr; // Pointer to last sent message (for retransmission)
|
|
|
|
// Queue for messages to be sent when token is received
|
|
osMessageQueueId_t queue_macData_id;
|
|
const osMessageQueueAttr_t queue_macData_attr = {
|
|
.name = "MAC_DATA"
|
|
};
|
|
|
|
/**
|
|
* @brief Send token to the next station
|
|
*/
|
|
void sendToken() {
|
|
// Create queueMsg struct with new memory
|
|
struct queueMsg_t queueMsg;
|
|
queueMsg.anyPtr = osMemoryPoolAlloc(memPool,osWaitForever);
|
|
queueMsg.type = TO_PHY;
|
|
|
|
// Copy token
|
|
memcpy(queueMsg.anyPtr, lastToken, TOKENSIZE-2);
|
|
|
|
// Send token to PHY
|
|
osStatus_t retCode = osMessageQueuePut(
|
|
queue_phyS_id,
|
|
&queueMsg,
|
|
osPriorityNormal,
|
|
0);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
}
|
|
|
|
/**
|
|
* @brief MAC sender task
|
|
*/
|
|
void MacSender(void *argument) {
|
|
struct queueMsg_t queueMsg; // queue message
|
|
uint8_t* msg; // pointer to message
|
|
Adresse src; // source Address
|
|
Adresse dst; // destination Address
|
|
uint8_t length; // length of message
|
|
Status status; // status of message
|
|
osStatus_t retCode; // return error code
|
|
char* strPtr; // pointer to string message
|
|
SapiToken stationStatus; // Status of one station
|
|
|
|
// Allocate memory for last token
|
|
lastToken = osMemoryPoolAlloc(memPool, osWaitForever);
|
|
|
|
// Create queue for messages to be sent when token is received
|
|
queue_macData_id = osMessageQueueNew(4, sizeof(struct queueMsg_t), &queue_macData_attr);
|
|
|
|
|
|
for(;;) {
|
|
//--------------------------------------------------------------------------
|
|
// QUEUE READ
|
|
//--------------------------------------------------------------------------
|
|
{
|
|
// Get message from queue, test retCode and get msg
|
|
retCode = osMessageQueueGet(
|
|
queue_macS_id,
|
|
&queueMsg,
|
|
NULL,
|
|
osWaitForever);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
|
|
msg = queueMsg.anyPtr;
|
|
}
|
|
|
|
switch(queueMsg.type) {
|
|
|
|
//----------------------------------------------------------------------
|
|
// TOKEN MESSAGE
|
|
//----------------------------------------------------------------------
|
|
case TOKEN: {
|
|
// Get token and save it
|
|
memcpy(lastToken, msg, TOKENSIZE-2);
|
|
|
|
// update token
|
|
lastToken[gTokenInterface.myAddress+1] = (0x1 << TIME_SAPI) + (gTokenInterface.connected << CHAT_SAPI);
|
|
for(uint8_t i = 1; i < sizeof(gTokenInterface.station_list); i++) {
|
|
gTokenInterface.station_list[i-1] = lastToken[i];
|
|
}
|
|
|
|
// send to lcd
|
|
queueMsg.type = TOKEN_LIST;
|
|
memcpy(queueMsg.anyPtr , lastToken, TOKENSIZE-2);
|
|
retCode = osMessageQueuePut(
|
|
queue_lcd_id,
|
|
&queueMsg,
|
|
osPriorityNormal,
|
|
osWaitForever);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
|
|
// Free memory
|
|
retCode = osMemoryPoolFree(memPool, queueMsg.anyPtr);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
|
|
// Send one msg from internal queue if exist
|
|
retCode = osMessageQueueGet(queue_macData_id, &queueMsg, NULL, 0);
|
|
if(retCode == 0){
|
|
queueMsg.type = TO_PHY;
|
|
retCode = osMessageQueuePut(
|
|
queue_phyS_id,
|
|
&queueMsg,
|
|
osPriorityNormal,
|
|
0);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
} else {
|
|
sendToken();
|
|
}
|
|
break;
|
|
}
|
|
|
|
//----------------------------------------------------------------------
|
|
// DATABACK MESSAGE
|
|
//----------------------------------------------------------------------
|
|
case DATABACK: {
|
|
// Get source Addresse, destination Addresse, length and status
|
|
src.raw = msg[0];
|
|
dst.raw = msg[1];
|
|
length = msg[2];
|
|
status.raw = msg[3+length];
|
|
|
|
if (dst.addr == BROADCAST_ADDRESS) {
|
|
// Broadcast message -> free memory
|
|
retCode = osMemoryPoolFree(memPool, queueMsg.anyPtr);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
|
|
// Send token
|
|
sendToken();
|
|
|
|
} else if(src.addr != gTokenInterface.myAddress) {
|
|
// Not from me -> to PHY
|
|
queueMsg.type = TO_PHY;
|
|
retCode = osMessageQueuePut(
|
|
queue_phyS_id,
|
|
&queueMsg,
|
|
osPriorityNormal,
|
|
0);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
|
|
} else if(status.read == 1) {
|
|
if(status.ack == 1) {
|
|
// Read + ack => Everything is fine -> free memory and send token
|
|
retCode = osMemoryPoolFree(memPool, queueMsg.anyPtr);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
retCode = osMemoryPoolFree(memPool, lastSentMsgPtr);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
sendToken();
|
|
|
|
} else {
|
|
// Read but checksum error, send original message again
|
|
if(lastSentMsgPtr != NULL) {
|
|
memcpy(queueMsg.anyPtr, lastSentMsgPtr, lastSentMsgPtr[2]+4);
|
|
queueMsg.type = TO_PHY;
|
|
//queueMsg.anyPtr = lastSentMsgPtr;
|
|
retCode = osMessageQueuePut(
|
|
queue_phyS_id,
|
|
&queueMsg,
|
|
osPriorityNormal,
|
|
0);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
|
|
} else {
|
|
// Error, no original message found
|
|
strPtr = osMemoryPoolAlloc(memPool, osWaitForever);
|
|
sprintf(strPtr, "%d did shit on the ring #1\0", dst.addr);
|
|
queueMsg.type = MAC_ERROR;
|
|
queueMsg.addr = src.addr;
|
|
queueMsg.sapi = src.sapi;
|
|
queueMsg.anyPtr = strPtr;
|
|
retCode = osMessageQueuePut(
|
|
queue_lcd_id,
|
|
&queueMsg,
|
|
osPriorityNormal,
|
|
0);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
}
|
|
}
|
|
|
|
} else {
|
|
// Not read => Station not connected -> free backup message
|
|
retCode = osMemoryPoolFree(memPool, queueMsg.anyPtr);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
|
|
// Send error message to LCD
|
|
strPtr = osMemoryPoolAlloc(memPool, osWaitForever);
|
|
sprintf(strPtr, "Dest. %d couldn't read message from %d\0", dst.addr+1, src.addr+1);
|
|
queueMsg.type = MAC_ERROR;
|
|
queueMsg.addr = src.addr;
|
|
queueMsg.sapi = src.sapi;
|
|
queueMsg.anyPtr = strPtr;
|
|
retCode = osMessageQueuePut(
|
|
queue_lcd_id,
|
|
&queueMsg,
|
|
osPriorityNormal,
|
|
0);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
|
|
// Send token
|
|
sendToken();
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
//----------------------------------------------------------------------
|
|
// NEW TOKEN MESSAGE
|
|
//----------------------------------------------------------------------
|
|
case NEW_TOKEN: {
|
|
// Create new token
|
|
lastToken[0] = TOKEN_TAG;
|
|
|
|
// Set all station status to 0
|
|
memset(lastToken, 0, sizeof(TOKENSIZE-2));
|
|
|
|
// Set my station status on station list and lastToken
|
|
gTokenInterface.station_list[gTokenInterface.myAddress] = (0x1 << TIME_SAPI) + (gTokenInterface.connected << CHAT_SAPI);
|
|
lastToken[gTokenInterface.myAddress+1] = gTokenInterface.station_list[gTokenInterface.myAddress];
|
|
|
|
// Send token
|
|
sendToken();
|
|
break;
|
|
}
|
|
|
|
//----------------------------------------------------------------------
|
|
// START MESSAGE
|
|
//----------------------------------------------------------------------
|
|
case START: {
|
|
gTokenInterface.connected = true;
|
|
break;
|
|
}
|
|
|
|
//----------------------------------------------------------------------
|
|
// STOP MESSAGE
|
|
//----------------------------------------------------------------------
|
|
case STOP: {
|
|
gTokenInterface.connected = false;
|
|
break;
|
|
}
|
|
|
|
//----------------------------------------------------------------------
|
|
// DATA MESSAGE
|
|
//----------------------------------------------------------------------
|
|
case DATA_IND: {
|
|
// Set source Addresse, destination Addresse and length
|
|
dst.addr = queueMsg.addr;
|
|
dst.sapi = queueMsg.sapi;
|
|
dst.nothing = 0;
|
|
src.addr = gTokenInterface.myAddress;
|
|
src.sapi = queueMsg.sapi;
|
|
src.nothing = 0;
|
|
length = strlen(queueMsg.anyPtr);
|
|
|
|
// Set station status
|
|
if(dst.addr == BROADCAST_ADDRESS) {
|
|
status.read = 1;
|
|
status.ack = 1;
|
|
stationStatus.raw = 0;
|
|
} else {
|
|
status.read = 0;
|
|
status.ack = 0;
|
|
stationStatus.raw = gTokenInterface.station_list[dst.addr];
|
|
}
|
|
|
|
// Check if destination is online
|
|
if( (dst.addr == BROADCAST_ADDRESS) || (stationStatus.chat == 1)) {
|
|
|
|
// Allocate memory for message and check if allocation was successful
|
|
msg = osMemoryPoolAlloc(memPool, 0);
|
|
if(msg == NULL) {
|
|
printf("Memory allocation failed #1\r\n");
|
|
assert(false);
|
|
}
|
|
|
|
// Set message
|
|
msg[0] = src.raw;
|
|
msg[1] = dst.raw;
|
|
msg[2] = length;
|
|
|
|
// Copy message to memory
|
|
memcpy(&msg[3], queueMsg.anyPtr, length);
|
|
|
|
// Set status
|
|
status.checksum = Checksum(msg);
|
|
msg[3+length] = status.raw;
|
|
|
|
// Free memory
|
|
retCode = osMemoryPoolFree(memPool, queueMsg.anyPtr);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
|
|
// Backup message if destination is chat station and isn't a broadcast message
|
|
if( (dst.addr != BROADCAST_ADDRESS) && (dst.sapi == CHAT_SAPI) ) {
|
|
lastSentMsgPtr = osMemoryPoolAlloc(memPool, 0);
|
|
if(lastSentMsgPtr == NULL) {
|
|
printf("Memory allocation failed #2\r\n");
|
|
assert(false);
|
|
}
|
|
memcpy(lastSentMsgPtr, msg, length+4);
|
|
}
|
|
|
|
// Send message to PHY
|
|
queueMsg.anyPtr = msg;
|
|
queueMsg.type = TO_PHY;
|
|
retCode = osMessageQueuePut(
|
|
queue_macData_id,
|
|
&queueMsg,
|
|
osPriorityNormal,
|
|
0);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
|
|
} else {
|
|
// Destination is not online
|
|
strPtr = queueMsg.anyPtr;
|
|
sprintf(strPtr, "%d is not online\0", dst.addr+1);
|
|
queueMsg.type = MAC_ERROR;
|
|
queueMsg.addr = src.addr;
|
|
queueMsg.anyPtr = strPtr;
|
|
retCode = osMessageQueuePut(
|
|
queue_lcd_id,
|
|
&queueMsg,
|
|
osPriorityNormal,
|
|
0);
|
|
CheckRetCode(retCode, __LINE__, __FILE__, CONTINUE);
|
|
}
|
|
|
|
break;
|
|
}
|
|
|
|
//----------------------------------------------------------------------
|
|
// DEFAULT - TBD
|
|
//----------------------------------------------------------------------
|
|
default: {
|
|
break;
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|