handle ID Control Frames

4 files changed, 140 insertions, 68 deletions

diff --git a/fw/can.c b/fw/can.c
index b3f41f9..a977a97 100644
--- a/fw/can.c
+++ b/fw/can.c
@@ -240,13 +240,13 @@ canIE(bool enable) {
 static void
 packId(CanId *id, U8 sidh, U8 sidl, U8 eid8, U8 eid0) {
 	if (sidl & IDE) { // extended ID
-		id->type = CAN_ID_EXT;
+		id->isExt = true;
 		id->eid[0] = (U8)(sidh << 3u) | (U8)(sidl >> 5u); // sid[7:0]
 		id->eid[1] = (U8)(eid0 << 3u) | (U8)(sidh >> 5u); //eid[4:0], sid[10:8]
 		id->eid[2] = (U8)(eid8 << 3u) | (U8)(eid0 >> 5u); // eid[12:5]
 		id->eid[3] = (U8)((sidl & 0x3) << 3u) | (U8)(eid8 >> 5u); // eid[17:13]
 	} else { // standard ID
-		id->type = CAN_ID_STD;
+		id->isExt = false;
 		id->sid.lo = (U8)(sidh << 3u) | (U8)(sidl >> 5u); // sid[7:0]
 		id->sid.hi = sidh >> 5u; // sid[10:8]
 	}
@@ -295,19 +295,16 @@ canReadRxb1(CanFrame *frame) {
 // e.g., RXMnSIDH etc.
 static void
 writeId(const CanId *id, Reg sidh, Reg sidl, Reg eid8, Reg eid0) {
-	switch (id->type) {
-	case CAN_ID_STD: // standard ID
-		write(sidh, (U8)(id->sid.hi << 5u) | (U8)(id->sid.lo >> 3u));
-		write(sidl, (U8) (id->sid.lo << 5u));
-		write(eid8, 0x00);
-		write(eid0, 0x00);
-		break;
-	case CAN_ID_EXT: // extended ID
+	if (id->isExt) { // extended
 		write(sidh, (U8)(id->eid[1] << 5u) | (U8)(id->eid[0] >> 3u)); // sid[10:3]
 		write(sidl, (U8)(id->eid[0] << 5u) | IDE | (U8)((id->eid[3] >> 3u) & 0x03)); // sid[2:0], exide, eid[28:27]
 		write(eid8, (U8)(id->eid[3] << 5u) | (U8)(id->eid[2] >> 3u)); // eid[26:19]
 		write(eid0, (U8)(id->eid[2] << 5u) | (U8)(id->eid[1] >> 3u)); // eid[18:11]
-		break;
+	} else { // standard
+		write(sidh, (U8)(id->sid.hi << 5u) | (U8)(id->sid.lo >> 3u));
+		write(sidl, (U8) (id->sid.lo << 5u));
+		write(eid8, 0x00);
+		write(eid0, 0x00);
 	}
 }
 
diff --git a/fw/can.h b/fw/can.h
index 9ac1cfe..196fb9f 100644
--- a/fw/can.h
+++ b/fw/can.h
@@ -39,10 +39,7 @@ typedef enum {
 
 // CAN identifier
 typedef struct {
-	enum {
-		CAN_ID_STD, // standard
-		CAN_ID_EXT, // extended
-	} type;
+	bool isExt; // is extended
 	union {
 		U16 sid; // 11-bit standard ID
 		U32 eid; // 29-bit extended ID
diff --git a/fw/eeprom.c b/fw/eeprom.c
index 71a53b1..11d0493 100644
--- a/fw/eeprom.c
+++ b/fw/eeprom.c
@@ -178,17 +178,12 @@ eepromWriteCanId(U16 addr, const CanId *id) {
 
 	// Copy ID to buffer
 	memset(buf, 0u, sizeof(buf));
-	switch (id->type) {
-	case CAN_ID_STD:
-		buf[0u] = id->sid.lo;
-		buf[1u] = id->sid.hi & 0x7;
-		break;
-	case CAN_ID_EXT:
+	if (id->isExt) { // extended
 		memmove(buf, id->eid, sizeof(buf));
 		buf[3u] = (buf[3u] & 0x1F) | 0x80; // set EID flag
-		break;
-	default:
-		return FAIL; // unreachable
+	} else { // standard
+		buf[0u] = id->sid.lo;
+		buf[1u] = id->sid.hi & 0x7;
 	}
 
 	return eepromWrite(addr, buf, sizeof(buf));
@@ -209,10 +204,10 @@ eepromReadCanId(U16 addr, CanId *id) {
 
 	// Unpack
 	if (id->eid[3u] & 0x80) { // bit 31 is standard/extended flag
-		id->type = CAN_ID_EXT; // extended
+		id->isExt = true; // extended
 		id->eid[3u] &= 0x1F;
 	} else {
-		id->type = CAN_ID_STD; // standard
+		id->isExt = false; // standard
 		id->sid.lo = id->eid[0u];
 		id->sid.hi = id->eid[1u] & 0x7;
 	}
diff --git a/fw/main.c b/fw/main.c
index 1f0308b..d5b1443 100644
--- a/fw/main.c
+++ b/fw/main.c
@@ -2,6 +2,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <string.h>
 
 #include "system.h"
 #include "types.h"
@@ -14,10 +15,22 @@
 // TODO: auto baud detection
 #define CAN_TIMING CAN_TIMING_10K
 
+// Parameters
+typedef enum {
+	TACH = 0,
+	SPEED,
+	AN1,
+	AN2,
+	AN3,
+	AN4,
+
+	NPARAM,
+} Param;
+
 // Table Control filter.
 // Used for writing/reading calibration tables.
 static const CanId tblCtrlFilter = {
-	.type = CAN_ID_EXT,
+	.isExt = true,
 	.eid = {0x00, 0x20, 0x27, 0x01}, // 12720XXh
 };
 
@@ -25,7 +38,7 @@ static const CanId tblCtrlFilter = {
 // Used for writing/reading the CAN ID associated with each parameter.
 // See `doc/datafmt.ps'.
 static const CanId idCtrlFilter = {
-	.type = CAN_ID_EXT,
+	.isExt = true,
 	.eid = {0x00, 0x21, 0x27, 0x01}, // 127210Xh
 };
 
@@ -34,7 +47,7 @@ static const CanId idCtrlFilter = {
 // and CAN IDs for each parameter.
 // This mask is the union of the two control message filters.
 static const CanId rxb0Mask = {
-	.type = CAN_ID_EXT,
+	.isExt = true,
 	.eid = {0x00, 0x21, 0x27, 0x01}, // 1272100h
 };
 
@@ -42,7 +55,7 @@ static const CanId rxb0Mask = {
 // RXB1 is used for receiving parameter values.
 // The mask is permissive: all messages are accepted and filtered in software.
 static const CanId rxb1Mask = {
-	.type = CAN_ID_EXT,
+	.isExt = true,
 	.eid = {0u, 0u, 0u, 0u}, // accept all messages
 };
 
@@ -58,53 +71,33 @@ static const Table an4Tbl = {{0x05, 0x00}};
 
 // EEPROM address of CAN ID for each parameter.
 // Each of these addresses holds a U32 CAN ID.
-static const EepromAddr tachIdAddr = {0x06, 0x00}; // tachometer
-static const EepromAddr speedIdAddr = {0x06, 0x04}; // speedometer
-static const EepromAddr an1IdAddr = {0x06, 0x08}; // analog channels...
-static const EepromAddr an2IdAddr = {0x06, 0x0C};
-static const EepromAddr an3IdAddr = {0x06, 0x10};
-static const EepromAddr an4IdAddr = {0x06, 0x14};
-
-static volatile CanId tachId; // tachometer
-static volatile CanId speedId; // speedometer
-static volatile CanId an1Id; // analog channels...
-static volatile CanId an2Id;
-static volatile CanId an3Id;
-static volatile CanId an4Id;
+static const EepromAddr paramIdAddrs[NPARAM] = {
+	[TACH] = {0x06, 0x00}, // tachometer
+	[SPEED] = {0x06, 0x04}, // speedometer
+	[AN1] = {0x06, 0x08}, // analog channels...
+	[AN2] = {0x06, 0x0C},
+	[AN3] = {0x06, 0x10},
+	[AN4] = {0x06, 0x14},
+};
+
+// CAN ID of each parameter
+static volatile CanId paramIds[NPARAM];
 
 // Load parameters' CAN IDs from EEPROM
 static Status
 loadParamIds(void) {
-	U8 oldGie;
+	U8 oldGie, k;
 	Status status;
 
 	// Disable interrupts so the volatile address pointers can be passed safely
 	oldGie = INTCONbits.GIE;
 	INTCONbits.GIE = 0;
 
-	status = eepromReadCanId(tachIdAddr, (CanId*)&tachId);
-	if (status != OK) {
-		return FAIL;
-	}
-	status = eepromReadCanId(speedIdAddr, (CanId*)&speedId);
-	if (status != OK) {
-		return FAIL;
-	}
-	status = eepromReadCanId(an1IdAddr, (CanId*)&an1Id);
-	if (status != OK) {
-		return FAIL;
-	}
-	status = eepromReadCanId(an2IdAddr, (CanId*)&an2Id);
-	if (status != OK) {
-		return FAIL;
-	}
-	status = eepromReadCanId(an3IdAddr, (CanId*)&an3Id);
-	if (status != OK) {
-		return FAIL;
-	}
-	status = eepromReadCanId(an4IdAddr, (CanId*)&an4Id);
-	if (status != OK) {
-		return FAIL;
+	for (k = 0u; k < NPARAM; k++) {
+		status = eepromReadCanId(paramIdAddrs[k], (CanId*)&paramIds[k]);
+		if (status != OK) {
+			return FAIL;
+		}
 	}
 
 	// Restore previous interrupt setting
@@ -163,11 +156,101 @@ handleTblCtrlFrame(const CanFrame *frame) {
 	// TODO
 }
 
+// Transmit the response to an ID Control REMOTE FRAME.
+// The response is an ID Control DATA FRAME containing the CAN ID
+// of the requested parameter.
+static Status
+respondIdCtrl(Param param) {
+	CanFrame response;
+	CanId paramId;
+	Status status;
+
+	// Read parameter's CAN ID from EEPROM
+	if ((U8)param < NPARAM) {
+		status = eepromReadCanId(paramIdAddrs[param], &paramId);
+		if (status != OK) {
+			return FAIL; // read failed
+		}
+	} else {
+		return FAIL; // invalid parameter
+	}
+
+	// Pack param ID into response's DATA FIELD
+	response.id = (CanId) {
+		.isExt = true,
+		.eid = {param & 0xF, 0x21, 0x27, 0x01}, // 127210Xh
+	};
+	response.rtr = false;
+	if (paramId.isExt) { // extended
+		response.dlc = 4u; // U32
+		// BE <- LE
+		response.data[0u] = paramId.eid[3u] & 0x1F;
+		response.data[1u] = paramId.eid[2u];
+		response.data[2u] = paramId.eid[1u];
+		response.data[3u] = paramId.eid[0u];
+	} else { // standard
+		response.dlc = 2u; // U16
+		response.data[0u] = paramId.sid.hi & 0x7;
+		response.data[1u] = paramId.sid.lo;
+	}
+
+	// Transmit response
+	return canTx(&response);
+}
+
+// Set the CAN ID associated with a parameter in response to an ID Control DATA FRAME.
+static Status
+setParamId(const CanFrame *frame) {
+	CanId paramId;
+	Param param;
+	Status status;
+
+	// Extract param ID from DATA FIELD
+	if (frame->dlc == 4u) { // extended
+		paramId.isExt = true;
+		// LE <- BE
+		paramId.eid[0u] = frame->data[3u];
+		paramId.eid[1u] = frame->data[2u];
+		paramId.eid[2u] = frame->data[1u];
+		paramId.eid[3u] = frame->data[0u] & 0x1F;
+	} else if (frame->dlc == 2u) { // standard
+		paramId.isExt = false;
+		paramId.sid.lo = frame->data[1u];
+		paramId.sid.hi = frame->data[0u] & 0x7;
+	} else {
+		return FAIL; // invalid DLC
+	}
+
+	// Extract param # from Control Frame's ID
+	param = frame->id.eid[0u] & 0xF;
+	if ((U8)param >= NPARAM) {
+		return FAIL; // invalid parameter
+	}
+
+	// Update copy in EEPROM
+	status = eepromWriteCanId(paramIdAddrs[param], &paramId);
+	if (status != OK) {
+		return FAIL; // write failed
+	}
+
+	// Update copy in PIC's RAM
+	paramIds[param] = paramId; 
+
+	return OK;
+}
+
 // Handle an ID Control Frame.
 // See `doc/datafmt.ps'
 static Status
 handleIdCtrlFrame(const CanFrame *frame) {
-	// TODO
+	Param param;
+
+	if (frame->rtr) { // REMOTE
+		param = frame->id.eid[0u] & 0xF;
+		return respondIdCtrl(param); // respond with the parameter's CAN ID
+	} else { // DATA
+		return setParamId(frame);
+	}
 }
 
 // Handle a frame potentially holding a parameter value.