From c38f18c2a32e3c91a9d0b1c18c0a52ee3d47f12e Mon Sep 17 00:00:00 2001 From: afanofosc Date: Wed, 3 Aug 2011 23:38:17 +0000 Subject: Fast I2C support Direct digital pin control support CommHSRead system call changes to support partial buffer reads git-svn-id: https://mindboards.svn.sourceforge.net/svnroot/mindboards/lms_nbcnxc/branches/version_131@42 c9361245-7fe8-9947-84e8-057757c4e366 --- AT91SAM7S256/Source/c_cmd.c | 167 ++++++++++---- AT91SAM7S256/Source/c_cmd.h | 3 +- AT91SAM7S256/Source/c_input.c | 223 +++++++++++------- AT91SAM7S256/Source/c_input.iom | 10 + AT91SAM7S256/Source/c_lowspeed.c | 137 +++++++---- AT91SAM7S256/Source/c_lowspeed.iom | 21 +- AT91SAM7S256/Source/c_output.c | 8 +- AT91SAM7S256/Source/d_input.c | 14 ++ AT91SAM7S256/Source/d_input.h | 2 + AT91SAM7S256/Source/d_input.r | 12 +- AT91SAM7S256/Source/d_loader.h | 4 +- AT91SAM7S256/Source/d_lowspeed.c | 6 + AT91SAM7S256/Source/d_lowspeed.h | 1 + AT91SAM7S256/Source/d_lowspeed.r | 452 +++++++++++++++++++++++++++++++++++++ 14 files changed, 892 insertions(+), 168 deletions(-) diff --git a/AT91SAM7S256/Source/c_cmd.c b/AT91SAM7S256/Source/c_cmd.c index 4a619f2..66b53f7 100644 --- a/AT91SAM7S256/Source/c_cmd.c +++ b/AT91SAM7S256/Source/c_cmd.c @@ -236,9 +236,9 @@ static pSysCall SysCallFuncs[SYSCALL_COUNT] = cCmdWrapUndefinedSysCall, cCmdWrapUndefinedSysCall, // 75 cCmdWrapUndefinedSysCall, - cCmdWrapUndefinedSysCall, // enhanced NBC/NXC - cCmdWrapIOMapReadByID, // 78 + cCmdWrapInputPinFunction, // 77 + cCmdWrapIOMapReadByID, cCmdWrapIOMapWriteByID, cCmdWrapDisplayExecuteFunction, // 80 cCmdWrapCommExecuteFunction, @@ -941,7 +941,9 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) case RC_LS_WRITE: { - i = pInBuf[1]; + i = (pInBuf[1] & 0x03); + UBYTE NoRestartOnRead = (pInBuf[1] & 0x04); + UBYTE bFast = (pInBuf[1] & 0x08); Count = pInBuf[2]; //Don't do anything if illegal port specification is made @@ -951,7 +953,7 @@ UWORD cCmdHandleRemoteCommands(UBYTE * pInBuf, UBYTE * pOutBuf, UBYTE * pLen) break; } - RCStatus = cCmdLSWrite(i, Count, &(pInBuf[4]), pInBuf[3], 0); + RCStatus = cCmdLSWrite(i, Count, &(pInBuf[4]), pInBuf[3], NoRestartOnRead, bFast); } break; @@ -3703,7 +3705,7 @@ NXT_STATUS cCmdColorSensorRead (UBYTE Port, SWORD * SensorValue, UWORD * RawArra UBYTE cCmdIsDSElementIDSane(DS_ELEMENT_ID Index) { - if (Index < VarsCmd.DataspaceCount) + if ((Index & DATA_ARG_IMM_MASK) < VarsCmd.DataspaceCount) return TRUE; else return FALSE; @@ -3964,8 +3966,12 @@ void* cCmdDSPtr(DS_ELEMENT_ID DSElementID, UWORD Offset) void * pDSItem; DV_INDEX DVIndex; TYPE_CODE TypeCode; + UBYTE bPointer = (DSElementID & 0x8000) != 0; NXT_ASSERT(cCmdIsDSElementIDSane(DSElementID)); + + DSElementID &= DATA_ARG_IMM_MASK; + // pointers are only valid if the type is UWORD TypeCode = cCmdDSType(DSElementID); if (TypeCode == TC_ARRAY) @@ -3987,8 +3993,17 @@ void* cCmdDSPtr(DS_ELEMENT_ID DSElementID, UWORD Offset) pDSItem = cCmdDSPtr(INC_ID(DSElementID), Offset); } else - pDSItem = (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset + Offset); - + { + if (bPointer && (TypeCode == TC_UWORD)) + { + pDSItem = (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset + Offset); + DSElementID = cCmdGetVal(pDSItem, TypeCode); + pDSItem = cCmdDSPtr(DSElementID, Offset); + } + else + pDSItem = (VarsCmd.pDataspace + VarsCmd.pDataspaceTOC[DSElementID].DSOffset + Offset); + } + NXT_ASSERT((UBYTE*)pDSItem < POOL_SENTINEL); return pDSItem; @@ -7702,7 +7717,7 @@ UBYTE cCmdLSCalcBytesReady(UBYTE Port) //cCmdLSWrite //Write BufLength bytes into specified port's lowspeed buffer and kick off comm process to device -NXT_STATUS cCmdLSWrite(UBYTE Port, UBYTE BufLength, UBYTE *pBuf, UBYTE ResponseLength, UBYTE NoRestartOnRead) +NXT_STATUS cCmdLSWrite(UBYTE Port, UBYTE BufLength, UBYTE *pBuf, UBYTE ResponseLength, UBYTE NoRestartOnRead, UBYTE bFast) { if (Port >= NO_OF_LOWSPEED_COM_CHANNEL) { @@ -7734,9 +7749,13 @@ NXT_STATUS cCmdLSWrite(UBYTE Port, UBYTE BufLength, UBYTE *pBuf, UBYTE ResponseL *pChState = LOWSPEED_INIT; pMapLowSpeed->State |= (COM_CHANNEL_ONE_ACTIVE << Port); if (NoRestartOnRead) - pMapLowSpeed->NoRestartOnRead |= (0x01 << Port); + pMapLowSpeed->NoRestartOnRead |= (COM_CHANNEL_NO_RESTART_1 << Port); + else + pMapLowSpeed->NoRestartOnRead &= ~(COM_CHANNEL_NO_RESTART_1 << Port); + if (bFast) + pMapLowSpeed->Speed |= (COM_CHANNEL_ONE_FAST << Port); else - pMapLowSpeed->NoRestartOnRead &= ~(0x01 << Port); + pMapLowSpeed->Speed &= ~(COM_CHANNEL_ONE_FAST << Port); return (NO_ERR); } @@ -8202,10 +8221,12 @@ NXT_STATUS cCmdWrapReadButton(UBYTE * ArgV[]) NXT_STATUS cCmdWrapCommLSWrite(UBYTE * ArgV[]) { SBYTE * pReturnVal = (SBYTE*)(ArgV[0]); - UBYTE Port = *(ArgV[1]); + UBYTE Port = (*(ArgV[1]) & 0x03); // 0..3 are valid port numbers UBYTE * pBuf; UWORD BufLength; UBYTE ResponseLength = *(ArgV[3]); + UBYTE NoRestartOnRead = (*(ArgV[1]) & 0x04); + UBYTE bFast = (*(ArgV[1]) & 0x08); DV_INDEX DVIndex; //Resolve array arguments @@ -8213,7 +8234,11 @@ NXT_STATUS cCmdWrapCommLSWrite(UBYTE * ArgV[]) pBuf = cCmdDVPtr(DVIndex); BufLength = DV_ARRAY[DVIndex].Count; - *pReturnVal = cCmdLSWrite(Port, (UBYTE)BufLength, pBuf, ResponseLength, 0); + *pReturnVal = cCmdLSWrite(Port, (UBYTE)BufLength, pBuf, ResponseLength, NoRestartOnRead, bFast); + if (bFast && (*pReturnVal == NO_ERR)) + *pReturnVal = pMapLowSpeed->pFunc(Port); + if (*pReturnVal >= NO_ERR) + *pReturnVal = NO_ERR; // returning a positive value causes problems in NXC API code that expects 0 (success) or non-zero error return (NO_ERR); } @@ -8589,8 +8614,8 @@ NXT_STATUS cCmdWrapCommBTWrite(UBYTE * ArgV[]) // //cCmdWrapCommBTRead //ArgV[0]: (return) Status byte, SBYTE -//ArgV[1]: Count to read -//ArgV[2]: Buffer +//ArgV[1]: Buffer +//ArgV[2]: Count to read // NXT_STATUS cCmdWrapCommBTRead(UBYTE * ArgV[]) { @@ -9589,35 +9614,78 @@ NXT_STATUS cCmdWrapCommHSWrite(UBYTE * ArgV[]) return (NO_ERR); } +//cCmdHSRead +//Read BufLength bytes from the hispeed buffer +NXT_STATUS cCmdHSRead(UBYTE BufLength, UBYTE * pBuf) +{ + UBYTE BytesReady, BytesToRead; + + if (BufLength > SIZE_OF_HSBUF) + { + return (ERR_INVALID_SIZE); + } + + BytesReady = cCmdHSCalcBytesReady(); + + if (BufLength > BytesReady) + { + return (ERR_COMM_CHAN_NOT_READY); + } + + BytesToRead = BufLength; + + HSBUF * pInBuf = &(pMapComm->HsInBuf); + + //If the bytes we want to read wrap around the end, we must first read the end, then reset back to the beginning + if (pInBuf->OutPtr + BytesToRead >= SIZE_OF_HSBUF) + { + BytesToRead = SIZE_OF_HSBUF - pInBuf->OutPtr; + memcpy(pBuf, pInBuf->Buf + pInBuf->OutPtr, BytesToRead); + pInBuf->OutPtr = 0; + pBuf += BytesToRead; + BytesToRead = BufLength - BytesToRead; + } + if (BytesToRead > 0) { + memcpy(pBuf, pInBuf->Buf + pInBuf->OutPtr, BytesToRead); + pInBuf->OutPtr += BytesToRead; + } + + return (NO_ERR); +} + //cCmdWrapCommHSRead //ArgV[0]: (return) Status byte, SBYTE //ArgV[1]: Buffer, out +//ArgV[2]: BufferLength, UBYTE, specifies size of buffer requested NXT_STATUS cCmdWrapCommHSRead(UBYTE * ArgV[]) { - UBYTE Tmp = cCmdHSCalcBytesReady(); - //Resolve array arguments - // output buffer + SBYTE * pReturnVal = (SBYTE*)(ArgV[0]); DV_INDEX DVIndex = *(DV_INDEX *)(ArgV[1]); - //Size Buffer to Length - NXT_STATUS Status = cCmdDVArrayAlloc(DVIndex, (UWORD)Tmp); - if (IS_ERR(Status)) - return Status; - UBYTE* pBuf = cCmdDVPtr(DVIndex); + UBYTE BufLength = *(ArgV[2]); + UBYTE BytesToRead; + NXT_STATUS Status; + UBYTE * pBuf; - //If the bytes we want to read wrap around the end, we must first read the end, then reset back to the beginning - UBYTE BytesToRead = Tmp; - if (pMapComm->HsInBuf.OutPtr + BytesToRead >= SIZE_OF_HSBUF) + BytesToRead = cCmdHSCalcBytesReady(); + + if (BytesToRead > 0) { - BytesToRead = SIZE_OF_HSBUF - pMapComm->HsInBuf.OutPtr; - memcpy(pBuf, pMapComm->HsInBuf.Buf + pMapComm->HsInBuf.OutPtr, BytesToRead); - pMapComm->HsInBuf.OutPtr = 0; - pBuf += BytesToRead; - BytesToRead = Tmp - BytesToRead; - } + //Limit buffer to available data + if (BufLength > BytesToRead) + BufLength = BytesToRead; - if (BytesToRead > 0) { - memcpy(pBuf, pMapComm->HsInBuf.Buf + pMapComm->HsInBuf.OutPtr, BytesToRead); - pMapComm->HsInBuf.OutPtr += BytesToRead; + Status = cCmdDVArrayAlloc(DVIndex, BufLength); + if (IS_ERR(Status)) + return (Status); + + pBuf = cCmdDVPtr(DVIndex); + *pReturnVal = cCmdHSRead(BufLength, pBuf); + } + else + { + Status = cCmdDVArrayAlloc(DVIndex, 0); + if (IS_ERR(Status)) + return (Status); } return (NO_ERR); @@ -9628,16 +9696,18 @@ NXT_STATUS cCmdWrapCommHSRead(UBYTE * ArgV[]) //ArgV[1]: Port specifier, UBYTE //ArgV[2]: Buffer to send, UBYTE array, only SIZE_OF_LSBUF bytes will be used //ArgV[3]: ResponseLength, UBYTE, specifies expected bytes back from slave device -//ArgV[4]: NoRestartOnRead, UBYTE, specifies whether or not to restart before the read +//ArgV[4]: Options, UBYTE, specifies whether or not to restart before the read and whether to use fast mode or not // NXT_STATUS cCmdWrapCommLSWriteEx(UBYTE * ArgV[]) { +/* SBYTE * pReturnVal = (SBYTE*)(ArgV[0]); UBYTE Port = *(ArgV[1]); UBYTE * pBuf; UWORD BufLength; UBYTE ResponseLength = *(ArgV[3]); - UBYTE NoRestartOnRead = *(ArgV[4]); + UBYTE NoRestartOnRead = *(ArgV[4]) & 0x01; + UBYTE bFast = *(ArgV[4]) & 0x02; DV_INDEX DVIndex; //Resolve array arguments @@ -9645,8 +9715,8 @@ NXT_STATUS cCmdWrapCommLSWriteEx(UBYTE * ArgV[]) pBuf = cCmdDVPtr(DVIndex); BufLength = DV_ARRAY[DVIndex].Count; - *pReturnVal = cCmdLSWrite(Port, (UBYTE)BufLength, pBuf, ResponseLength, NoRestartOnRead); - + *pReturnVal = cCmdLSWrite(Port, (UBYTE)BufLength, pBuf, ResponseLength, NoRestartOnRead, bFast); +*/ return (NO_ERR); } @@ -9800,6 +9870,27 @@ NXT_STATUS cCmdWrapRandomEx(UBYTE * ArgV[]) return NO_ERR; } + +// +//cCmdWrapInputPinFunction +//ArgV[0]: (return) Result word, UWORD +//ArgV[1]: UBYTE Cmd +//ArgV[2]: UBYTE Port +//ArgV[3]: UBYTE Pin +//ArgV[4]: UBYTE pData +// +NXT_STATUS cCmdWrapInputPinFunction(UBYTE * ArgV[]) +{ + *(UWORD*)(ArgV[0]) = + pMapInput->pFunc(*(UBYTE*)(ArgV[1]), + *(UBYTE*)(ArgV[2]), + *(UBYTE*)(ArgV[3]), + (UBYTE*)(ArgV[4]) + ); + return (NO_ERR); +} + + NXT_STATUS cCmdWrapUndefinedSysCall(UBYTE * ArgV[]) { return (NO_ERR); diff --git a/AT91SAM7S256/Source/c_cmd.h b/AT91SAM7S256/Source/c_cmd.h index 22f1a2e..d857581 100644 --- a/AT91SAM7S256/Source/c_cmd.h +++ b/AT91SAM7S256/Source/c_cmd.h @@ -827,7 +827,7 @@ float cCmdGetValFlt(void * pVal, TYPE_CODE TypeCode); NXT_STATUS cCmdLSCheckStatus(UBYTE Port); UBYTE cCmdLSCalcBytesReady(UBYTE Port); -NXT_STATUS cCmdLSWrite(UBYTE Port, UBYTE BufLength, UBYTE *pBuf, UBYTE ResponseLength, UBYTE NoRestartOnRead); +NXT_STATUS cCmdLSWrite(UBYTE Port, UBYTE BufLength, UBYTE *pBuf, UBYTE ResponseLength, UBYTE NoRestartOnRead, UBYTE bFast); NXT_STATUS cCmdLSRead(UBYTE Port, UBYTE BufLength, UBYTE * pBuf); // @@ -892,6 +892,7 @@ NXT_STATUS cCmdWrapWriteSemData(UBYTE * ArgV[]); NXT_STATUS cCmdWrapUpdateCalibCacheInfo(UBYTE * ArgV[]); NXT_STATUS cCmdWrapComputeCalibValue(UBYTE * ArgV[]); +NXT_STATUS cCmdWrapInputPinFunction(UBYTE * ArgV[]); NXT_STATUS cCmdWrapIOMapReadByID(UBYTE * ArgV[]); NXT_STATUS cCmdWrapIOMapWriteByID(UBYTE * ArgV[]); NXT_STATUS cCmdWrapDisplayExecuteFunction(UBYTE * ArgV[]); diff --git a/AT91SAM7S256/Source/c_input.c b/AT91SAM7S256/Source/c_input.c index d60f850..319bad4 100644 --- a/AT91SAM7S256/Source/c_input.c +++ b/AT91SAM7S256/Source/c_input.c @@ -17,6 +17,7 @@ #include "modules.h" #include "c_input.h" #include "d_input.h" +#include "c_cmd.iom" #include "c_output.iom" #include "c_loader.iom" #include @@ -150,9 +151,11 @@ void cInputSetupCustomSensor(UBYTE Port); void cInputCalcSensorValues(UBYTE No); UBYTE cInputInitColorSensor(UBYTE Port, UBYTE *pInitStatus); void cInputCalibrateColor(COLORSTRUCT *pC, UWORD *pNewVals); +UBYTE cInputPinFunc(UBYTE Cmd, UBYTE Port, UBYTE Pin, UBYTE *pData); void cInputInit(void* pHeader) { + IOMapInput.pFunc = &cInputPinFunc; UBYTE Tmp; memset(IOMapInput.Colors, 0, sizeof(IOMapInput.Colors)); @@ -161,19 +164,20 @@ void cInputInit(void* pHeader) /* Init IO map */ for (Tmp = 0; Tmp < NO_OF_INPUTS; Tmp++) { - IOMapInput.Inputs[Tmp].SensorType = NO_SENSOR; - IOMapInput.Inputs[Tmp].SensorMode = RAWMODE; - IOMapInput.Inputs[Tmp].SensorRaw = 0; - IOMapInput.Inputs[Tmp].SensorValue = 0; - IOMapInput.Inputs[Tmp].SensorBoolean = 0; - IOMapInput.Inputs[Tmp].InvalidData = INVALID_DATA; - IOMapInput.Inputs[Tmp].DigiPinsDir = 0; - IOMapInput.Inputs[Tmp].DigiPinsOut = 0; - IOMapInput.Inputs[Tmp].CustomActiveStatus = CUSTOMINACTIVE; - IOMapInput.Inputs[Tmp].CustomZeroOffset = 0; - IOMapInput.Inputs[Tmp].CustomPctFullScale = 0; - dInputRead0(Tmp, &(IOMapInput.Inputs[Tmp].DigiPinsIn)); - dInputRead1(Tmp, &(IOMapInput.Inputs[Tmp].DigiPinsIn)); + INPUTSTRUCT * pIn = &(IOMapInput.Inputs[Tmp]); + pIn->SensorType = NO_SENSOR; + pIn->SensorMode = RAWMODE; + pIn->SensorRaw = 0; + pIn->SensorValue = 0; + pIn->SensorBoolean = 0; + pIn->InvalidData = INVALID_DATA; + pIn->DigiPinsDir = 0; + pIn->DigiPinsOut = 0; + pIn->CustomActiveStatus = CUSTOMINACTIVE; + pIn->CustomZeroOffset = 0; + pIn->CustomPctFullScale = 0; + dInputRead0(Tmp, &(pIn->DigiPinsIn)); + dInputRead1(Tmp, &(pIn->DigiPinsIn)); VarsInput.EdgeCnt[Tmp] = 0; VarsInput.InputDebounce[Tmp] = 0; @@ -226,7 +230,8 @@ void cInputCtrl(void) for (Tmp = 0; Tmp < NO_OF_INPUTS; Tmp++) { - UBYTE sType = IOMapInput.Inputs[Tmp].SensorType; + INPUTSTRUCT * pIn = &(IOMapInput.Inputs[Tmp]); + UBYTE sType = pIn->SensorType; UBYTE oldType = VarsInput.OldSensorType[Tmp]; if (sType != oldType) @@ -248,12 +253,12 @@ void cInputCtrl(void) oldType == COLOREXIT)) { VarsInput.InvalidTimer[Tmp] = INVALID_RELOAD_COLOR; - IOMapInput.Inputs[Tmp].SensorType = COLOREXIT; + pIn->SensorType = COLOREXIT; sType = COLOREXIT; } /* Setup the pins for the new sensortype */ cInputSetupType(Tmp, sType, oldType); - IOMapInput.Inputs[Tmp].InvalidData = INVALID_DATA; + pIn->InvalidData = INVALID_DATA; VarsInput.OldSensorType[Tmp] = sType; } else @@ -278,7 +283,7 @@ void cInputCtrl(void) { /* Time elapsed - data are now valid */ - IOMapInput.Inputs[Tmp].InvalidData &= ~INVALID_DATA; + pIn->InvalidData &= ~INVALID_DATA; } } else @@ -286,11 +291,11 @@ void cInputCtrl(void) /* The invalid bit could have been set by the VM due to Mode change */ /* but input module needs to be called once to update the values */ - IOMapInput.Inputs[Tmp].InvalidData &= ~INVALID_DATA; + pIn->InvalidData &= ~INVALID_DATA; } } - if (!(INVALID_DATA & (IOMapInput.Inputs[Tmp].InvalidData))) + if (!(INVALID_DATA & (pIn->InvalidData))) { cInputCalcSensorValues(Tmp); } @@ -300,7 +305,8 @@ void cInputCtrl(void) void cInputCalcSensorValues(UBYTE No) { - UBYTE sType = IOMapInput.Inputs[No].SensorType; + INPUTSTRUCT * pIn = &(IOMapInput.Inputs[No]); + UBYTE sType = pIn->SensorType; switch(sType) { @@ -319,12 +325,12 @@ void cInputCalcSensorValues(UBYTE No) if (sType == CUSTOM) { /* Setup and read digital IO */ cInputSetupCustomSensor(No); - dInputRead0(No, &(IOMapInput.Inputs[No].DigiPinsIn)); - dInputRead1(No, &(IOMapInput.Inputs[No].DigiPinsIn)); + dInputRead0(No, &(pIn->DigiPinsIn)); + dInputRead1(No, &(pIn->DigiPinsIn)); } dInputGetRawAd(&InputVal, No); - IOMapInput.Inputs[No].ADRaw = InputVal; + pIn->ADRaw = InputVal; if (sType == REFLECTION) { @@ -350,27 +356,30 @@ void cInputCalcSensorValues(UBYTE No) } else if (sType == CUSTOM) { - cInputCalcFullScale(&InputVal, IOMapInput.Inputs[No].CustomZeroOffset, IOMapInput.Inputs[No].CustomPctFullScale, FALSE); + cInputCalcFullScale(&InputVal, pIn->CustomZeroOffset, pIn->CustomPctFullScale, FALSE); } cInputCalcSensorValue( InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), + &(pIn->SensorRaw), + &(pIn->SensorValue), + &(pIn->SensorBoolean), &(VarsInput.InputDebounce[No]), &(VarsInput.SampleCnt[No]), &(VarsInput.LastAngle[No]), &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); + ((pIn->SensorMode) & SLOPEMASK), + ((pIn->SensorMode) & MODEMASK)); } break; - /* Tripple case intended */ + /* Triple case intended */ case LOWSPEED: case LOWSPEED_9V: case HIGHSPEED: { + UWORD InputVal; + dInputGetRawAd(&InputVal, No); + pIn->ADRaw = InputVal; } break; @@ -380,9 +389,10 @@ void cInputCalcSensorValues(UBYTE No) case COLORBLUE: case COLORNONE: { + COLORSTRUCT * pC = &(IOMapInput.Colors[No]); UWORD InputVal; - switch (IOMapInput.Colors[No].CalibrationState) + switch (pC->CalibrationState) { case SENSOROFF: { @@ -392,7 +402,7 @@ void cInputCalcSensorValues(UBYTE No) /* Sensor has been attached now get cal data */ VarsInput.VarsColor[No].ColorInitState = 0; - (IOMapInput.Colors[No].CalibrationState) = SENSORCAL; + (pC->CalibrationState) = SENSORCAL; } } break; @@ -404,7 +414,7 @@ void cInputCalcSensorValues(UBYTE No) { /* Color sensor has been removed during calibration */ - (IOMapInput.Colors[No].CalibrationState) = SENSOROFF; + (pC->CalibrationState) = SENSOROFF; } if (TRUE == Status) @@ -412,30 +422,30 @@ void cInputCalcSensorValues(UBYTE No) /* Use clock to detect errors */ dInputSetDirInDigi0(No); - (IOMapInput.Colors[No].CalibrationState) = 0; + (pC->CalibrationState) = 0; } } break; default: { - if (dInputGetColor(No, &(IOMapInput.Inputs[No].ADRaw))) + if (dInputGetColor(No, &(pIn->ADRaw))) { - InputVal = IOMapInput.Inputs[No].ADRaw; + InputVal = pIn->ADRaw; cInputCalcFullScale(&InputVal, COLORSENSORBGMIN, COLORSENSORBGPCTDYN, FALSE); cInputCalcSensorValue(InputVal, - &(IOMapInput.Inputs[No].SensorRaw), - &(IOMapInput.Inputs[No].SensorValue), - &(IOMapInput.Inputs[No].SensorBoolean), + &(pIn->SensorRaw), + &(pIn->SensorValue), + &(pIn->SensorBoolean), &(VarsInput.InputDebounce[No]), &(VarsInput.SampleCnt[No]), &(VarsInput.LastAngle[No]), &(VarsInput.EdgeCnt[No]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); + ((pIn->SensorMode) & SLOPEMASK), + ((pIn->SensorMode) & MODEMASK)); } else { - IOMapInput.Colors[No].CalibrationState = SENSOROFF; + pC->CalibrationState = SENSOROFF; } } break; @@ -444,7 +454,8 @@ void cInputCalcSensorValues(UBYTE No) break; case COLORFULL: { - switch (IOMapInput.Colors[No].CalibrationState) + COLORSTRUCT * pC = &(IOMapInput.Colors[No]); + switch (pC->CalibrationState) { case SENSOROFF: { @@ -454,7 +465,7 @@ void cInputCalcSensorValues(UBYTE No) /* Sensor has been attached now get cal data */ VarsInput.VarsColor[No].ColorInitState = 0; - (IOMapInput.Colors[No].CalibrationState) = SENSORCAL; + (pC->CalibrationState) = SENSORCAL; } } break; @@ -466,7 +477,7 @@ void cInputCalcSensorValues(UBYTE No) { /* Color sensor has been removed during calibration */ - (IOMapInput.Colors[No].CalibrationState) = SENSOROFF; + (pC->CalibrationState) = SENSOROFF; VarsInput.ColorStatus &= ~(0x01<CalibrationState) = 0; /* Calculate Calibration factor */ VarsInput.ColorStatus |= (0x01<SensorRaw[ColorCount]), + &(pC->SensorValue[ColorCount]), + &(pC->Boolean[ColorCount]), &(VarsInput.VarsColor[No].ColorInputDebounce[ColorCount]), &(VarsInput.VarsColor[No].ColorSampleCnt[ColorCount]), &(VarsInput.VarsColor[No].ColorLastAngle[ColorCount]), &(VarsInput.VarsColor[No].ColorEdgeCnt[ColorCount]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); + ((pIn->SensorMode) & SLOPEMASK), + ((pIn->SensorMode) & MODEMASK)); } /* Calculate background sensor values */ cInputCalcSensorValue(NewSensorVals[BLANK], - &(IOMapInput.Colors[No].SensorRaw[BLANK]), - &(IOMapInput.Colors[No].SensorValue[BLANK]), - &(IOMapInput.Colors[No].Boolean[BLANK]), + &(pC->SensorRaw[BLANK]), + &(pC->SensorValue[BLANK]), + &(pC->Boolean[BLANK]), &(VarsInput.VarsColor[No].ColorInputDebounce[BLANK]), &(VarsInput.VarsColor[No].ColorSampleCnt[BLANK]), &(VarsInput.VarsColor[No].ColorLastAngle[BLANK]), &(VarsInput.VarsColor[No].ColorEdgeCnt[BLANK]), - ((IOMapInput.Inputs[No].SensorMode) & SLOPEMASK), - ((IOMapInput.Inputs[No].SensorMode) & MODEMASK)); + ((pIn->SensorMode) & SLOPEMASK), + ((pIn->SensorMode) & MODEMASK)); /* Color Sensor values has been calculated - */ /* now calculate the color and put it in Sensor value */ @@ -541,14 +548,14 @@ void cInputCalcSensorValues(UBYTE No) if (((pC->SensorRaw[RED]) < 65) || (((pC->SensorRaw[BLANK]) < 40) && ((pC->SensorRaw[RED]) < 110))) { - IOMapInput.Inputs[No].SensorValue = BLACKCOLOR; + pIn->SensorValue = BLACKCOLOR; } else { if (((((pC->SensorRaw[BLUE]) >> 2) + ((pC->SensorRaw[BLUE]) >> 3) + (pC->SensorRaw[BLUE])) < (pC->SensorRaw[GREEN])) && ((((pC->SensorRaw[GREEN]) << 1)) > (pC->SensorRaw[RED]))) { - IOMapInput.Inputs[No].SensorValue = YELLOWCOLOR; + pIn->SensorValue = YELLOWCOLOR; } else { @@ -556,7 +563,7 @@ void cInputCalcSensorValues(UBYTE No) if ((((pC->SensorRaw[GREEN]) << 1) - ((pC->SensorRaw[GREEN]) >> 2)) < (pC->SensorRaw[RED])) { - IOMapInput.Inputs[No].SensorValue = REDCOLOR; + pIn->SensorValue = REDCOLOR; } else { @@ -565,11 +572,11 @@ void cInputCalcSensorValues(UBYTE No) ((pC->SensorRaw[GREEN]) < 70)) || (((pC->SensorRaw[BLANK]) < 140) && ((pC->SensorRaw[RED]) < 140))) { - IOMapInput.Inputs[No].SensorValue = BLACKCOLOR; + pIn->SensorValue = BLACKCOLOR; } else { - IOMapInput.Inputs[No].SensorValue = WHITECOLOR; + pIn->SensorValue = WHITECOLOR; } } } @@ -587,20 +594,20 @@ void cInputCalcSensorValues(UBYTE No) if (((pC->SensorRaw[GREEN]) < 40) || (((pC->SensorRaw[BLANK]) < 30) && ((pC->SensorRaw[GREEN]) < 70))) { - IOMapInput.Inputs[No].SensorValue = BLACKCOLOR; + pIn->SensorValue = BLACKCOLOR; } else { if ((((pC->SensorRaw[BLUE]) << 1)) < (pC->SensorRaw[RED])) { - IOMapInput.Inputs[No].SensorValue = YELLOWCOLOR; + pIn->SensorValue = YELLOWCOLOR; } else { if ((((pC->SensorRaw[RED]) + ((pC->SensorRaw[RED])>>2)) < (pC->SensorRaw[GREEN])) || (((pC->SensorRaw[BLUE]) + ((pC->SensorRaw[BLUE])>>2)) < (pC->SensorRaw[GREEN]))) { - IOMapInput.Inputs[No].SensorValue = GREENCOLOR; + pIn->SensorValue = GREENCOLOR; } else { @@ -608,11 +615,11 @@ void cInputCalcSensorValues(UBYTE No) ((pC->SensorRaw[BLUE]) < 70)) || (((pC->SensorRaw[BLANK]) < 140) && ((pC->SensorRaw[GREEN]) < 140))) { - IOMapInput.Inputs[No].SensorValue = BLACKCOLOR; + pIn->SensorValue = BLACKCOLOR; } else { - IOMapInput.Inputs[No].SensorValue = WHITECOLOR; + pIn->SensorValue = WHITECOLOR; } } } @@ -627,7 +634,7 @@ void cInputCalcSensorValues(UBYTE No) if (((pC->SensorRaw[BLUE]) < 48) || (((pC->SensorRaw[BLANK]) < 25) && ((pC->SensorRaw[BLUE]) < 85))) { - IOMapInput.Inputs[No].SensorValue = BLACKCOLOR; + pIn->SensorValue = BLACKCOLOR; } else { @@ -637,7 +644,7 @@ void cInputCalcSensorValues(UBYTE No) (((((pC->SensorRaw[RED]) * 58) >> 5) < (pC->SensorRaw[BLUE])) || ((((pC->SensorRaw[GREEN]) * 58) >> 5) < (pC->SensorRaw[BLUE])))) { - IOMapInput.Inputs[No].SensorValue = BLUECOLOR; + pIn->SensorValue = BLUECOLOR; } else { @@ -647,18 +654,18 @@ void cInputCalcSensorValues(UBYTE No) ((pC->SensorRaw[GREEN]) < 60)) || (((pC->SensorRaw[BLANK]) < 110) && ((pC->SensorRaw[BLUE]) < 120))) { - IOMapInput.Inputs[No].SensorValue = BLACKCOLOR; + pIn->SensorValue = BLACKCOLOR; } else { if ((((pC->SensorRaw[RED]) + ((pC->SensorRaw[RED]) >> 3)) < (pC->SensorRaw[BLUE])) || (((pC->SensorRaw[GREEN]) + ((pC->SensorRaw[GREEN]) >> 3)) < (pC->SensorRaw[BLUE]))) { - IOMapInput.Inputs[No].SensorValue = BLUECOLOR; + pIn->SensorValue = BLUECOLOR; } else { - IOMapInput.Inputs[No].SensorValue = WHITECOLOR; + pIn->SensorValue = WHITECOLOR; } } } @@ -668,7 +675,7 @@ void cInputCalcSensorValues(UBYTE No) } else { - IOMapInput.Colors[No].CalibrationState = SENSOROFF; + pC->CalibrationState = SENSOROFF; VarsInput.ColorStatus &= ~(0x01<SensorType = NO_SENSOR; } if (TRUE == Status) @@ -692,7 +699,7 @@ void cInputCalcSensorValues(UBYTE No) /* Initialization finished with success recalc the values*/ (IOMapInput.Colors[No].CalibrationState) = 0; - IOMapInput.Inputs[No].SensorType = NO_SENSOR; + pIn->SensorType = NO_SENSOR; VarsInput.OldSensorType[No] = NO_SENSOR; } } @@ -1363,3 +1370,63 @@ void cInputExit(void) dInputExit(); } +UBYTE cInputPinFunc(UBYTE Cmd, UBYTE Port, UBYTE Pin, UBYTE *pData) +{ + UBYTE ReturnState = NO_ERR; + if (IOMapInput.Inputs[Port].SensorType != CUSTOM) + return (UBYTE)ERR_INVALID_PORT; + + UBYTE WaitUSEC = (Cmd&0xFC)>>2; + UBYTE Dir = (Pin&0xFC)>>2; + Pin &= 0x03; + + switch(Cmd&0x03) + { + case PINDIR: + { + if (Pin & DIGI0) + { + if (Dir) + dInputSetDirInDigi0(Port); + else + dInputSetDirOutDigi0(Port); + } + if (Pin & DIGI1) + { + if (Dir) + dInputSetDirInDigi1(Port); + else + dInputSetDirOutDigi1(Port); + } + } + break; + case SETPIN: + { + if (Pin & DIGI0) + dInputSetDigi0(Port); + if (Pin & DIGI1) + dInputSetDigi1(Port); + } + break; + case CLEARPIN: + { + if (Pin & DIGI0) + dInputClearDigi0(Port); + if (Pin & DIGI1) + dInputClearDigi1(Port); + } + break; + case READPIN: + { + if (Pin & DIGI0) + dInputRead0(Port, pData); + if (Pin & DIGI1) + dInputRead1(Port, pData); + } + break; + } + if (WaitUSEC) + dInputWaitUS(WaitUSEC); + + return (ReturnState); +} diff --git a/AT91SAM7S256/Source/c_input.iom b/AT91SAM7S256/Source/c_input.iom index dee1309..b317f54 100644 --- a/AT91SAM7S256/Source/c_input.iom +++ b/AT91SAM7S256/Source/c_input.iom @@ -78,6 +78,15 @@ enum INVALID_DATA = 0x01 }; +/* Constants related to calling cInputPinFunc */ +enum +{ + PINDIR, + SETPIN, + CLEARPIN, + READPIN +}; + /* Constants related to Colorstruct */ enum { @@ -167,6 +176,7 @@ typedef struct { INPUTSTRUCT Inputs[NO_OF_INPUTS]; COLORSTRUCT Colors[NO_OF_INPUTS]; + UBYTE (*pFunc)(UBYTE, UBYTE, UBYTE, UBYTE *); }IOMAPINPUT; #endif diff --git a/AT91SAM7S256/Source/c_lowspeed.c b/AT91SAM7S256/Source/c_lowspeed.c index f8baa92..658b5fc 100644 --- a/AT91SAM7S256/Source/c_lowspeed.c +++ b/AT91SAM7S256/Source/c_lowspeed.c @@ -18,6 +18,7 @@ #include "c_input.iom" #include "c_lowspeed.h" #include "d_lowspeed.h" +#include static IOMAPLOWSPEED IOMapLowSpeed; static VARSLOWSPEED VarsLowSpeed; @@ -39,15 +40,89 @@ const HEADER cLowSpeed = 0x0000 //Code size - not used so far }; +SBYTE cLowSpeedFastI2C(UBYTE ch); + void cLowSpeedInit(void* pHeader) { pHeaders = pHeader; dLowSpeedInit(); IOMapLowSpeed.State = COM_CHANNEL_NONE_ACTIVE; + IOMapLowSpeed.NoRestartOnRead = COM_CHANNEL_RESTART_ALL; + IOMapLowSpeed.Speed = COM_CHANNEL_NONE_FAST; + IOMapLowSpeed.pFunc = &cLowSpeedFastI2C; VarsLowSpeed.TimerState = TIMER_STOPPED; } +void cLowSpeedCompleteRead(UBYTE ch) +{ + for (VarsLowSpeed.InputBuf[ch].OutPtr = 0; VarsLowSpeed.InputBuf[ch].OutPtr < IOMapLowSpeed.InBuf[ch].BytesToRx; VarsLowSpeed.InputBuf[ch].OutPtr++) + { + IOMapLowSpeed.InBuf[ch].Buf[IOMapLowSpeed.InBuf[ch].InPtr] = VarsLowSpeed.InputBuf[ch].Buf[VarsLowSpeed.InputBuf[ch].OutPtr]; + IOMapLowSpeed.InBuf[ch].InPtr++; + if (IOMapLowSpeed.InBuf[ch].InPtr >= SIZE_OF_LSBUF) + { + IOMapLowSpeed.InBuf[ch].InPtr = 0; + } + VarsLowSpeed.InputBuf[ch].Buf[VarsLowSpeed.InputBuf[ch].OutPtr] = 0; + } +} + +void cLowSpeedLoadWriteBuffer(UBYTE ch) +{ + VarsLowSpeed.OutputBuf[ch].OutPtr = 0; + memcpy(VarsLowSpeed.OutputBuf[ch].Buf, IOMapLowSpeed.OutBuf[ch].Buf, IOMapLowSpeed.OutBuf[ch].InPtr); + VarsLowSpeed.OutputBuf[ch].InPtr = IOMapLowSpeed.OutBuf[ch].InPtr; + IOMapLowSpeed.OutBuf[ch].OutPtr = IOMapLowSpeed.OutBuf[ch].InPtr; +/* + VarsLowSpeed.OutputBuf[ch].OutPtr = 0; + for (VarsLowSpeed.OutputBuf[ch].InPtr = 0; VarsLowSpeed.OutputBuf[ch].InPtr < IOMapLowSpeed.OutBuf[ch].InPtr; VarsLowSpeed.OutputBuf[ch].InPtr++) + { + VarsLowSpeed.OutputBuf[ch].Buf[VarsLowSpeed.OutputBuf[ch].InPtr] = IOMapLowSpeed.OutBuf[ch].Buf[IOMapLowSpeed.OutBuf[ch].OutPtr]; + IOMapLowSpeed.OutBuf[ch].OutPtr++; + } +*/ +} + +void cLowSpeedFinished(UBYTE ch, UBYTE bDone) +{ + IOMapLowSpeed.State = IOMapLowSpeed.State & ~LOWSPEED_CH_NUMBER[ch]; + if (bDone) + IOMapLowSpeed.ChannelState[ch] = LOWSPEED_IDLE; + if (IOMapLowSpeed.State == 0) + { + dLowSpeedStopTimer(); + VarsLowSpeed.TimerState = TIMER_STOPPED; + } +} + + +SBYTE cLowSpeedFastI2C(UBYTE ch) +{ + cLowSpeedLoadWriteBuffer(ch); + SBYTE result = dLowSpeedFastI2C(ch, + *(VarsLowSpeed.OutputBuf[ch].Buf), + VarsLowSpeed.OutputBuf[ch].Buf+1, + VarsLowSpeed.OutputBuf[ch].InPtr-1, + &(IOMapLowSpeed.InBuf[ch].BytesToRx), + VarsLowSpeed.InputBuf[ch].Buf); + if (result >= 0) + { + // finally copy the data from the VarsLowSpeed buffer into the IOMapLowSpeed buffer + // and update our channel state and mode + cLowSpeedCompleteRead(ch); + IOMapLowSpeed.Mode[ch] = LOWSPEED_DATA_RECEIVED; +// IOMapLowSpeed.ChannelState[ch] = LOWSPEED_DONE; + cLowSpeedFinished(ch, TRUE); + } + else + { + IOMapLowSpeed.ChannelState[ch] = LOWSPEED_ERROR; + IOMapLowSpeed.ErrorType[ch] = (UBYTE)result; + } + return result; +} + void cLowSpeedCtrl(void) { UBYTE Temp; @@ -69,15 +144,22 @@ void cLowSpeedCtrl(void) { if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) { - if (VarsLowSpeed.TimerState == TIMER_STOPPED) - { - dLowSpeedStartTimer(); - VarsLowSpeed.TimerState = TIMER_RUNNING; - } - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_LOAD_BUFFER; - IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_NO_ERROR; - VarsLowSpeed.ErrorCount[ChannelNumber] = 0; - dLowSpeedInitPins(ChannelNumber); + if (IOMapLowSpeed.Speed & (COM_CHANNEL_ONE_FAST << ChannelNumber)) + { + cLowSpeedFastI2C(ChannelNumber); + } + else + { + if (VarsLowSpeed.TimerState == TIMER_STOPPED) + { + dLowSpeedStartTimer(); + VarsLowSpeed.TimerState = TIMER_RUNNING; + } + IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_LOAD_BUFFER; + IOMapLowSpeed.ErrorType[ChannelNumber] = LOWSPEED_NO_ERROR; + VarsLowSpeed.ErrorCount[ChannelNumber] = 0; + dLowSpeedInitPins(ChannelNumber); + } } else { @@ -91,13 +173,8 @@ void cLowSpeedCtrl(void) { if ((pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED) || (pMapInput->Inputs[ChannelNumber].SensorType == LOWSPEED_9V)) { - VarsLowSpeed.OutputBuf[ChannelNumber].OutPtr = 0; - for (VarsLowSpeed.OutputBuf[ChannelNumber].InPtr = 0; VarsLowSpeed.OutputBuf[ChannelNumber].InPtr < IOMapLowSpeed.OutBuf[ChannelNumber].InPtr; VarsLowSpeed.OutputBuf[ChannelNumber].InPtr++) - { - VarsLowSpeed.OutputBuf[ChannelNumber].Buf[VarsLowSpeed.OutputBuf[ChannelNumber].InPtr] = IOMapLowSpeed.OutBuf[ChannelNumber].Buf[IOMapLowSpeed.OutBuf[ChannelNumber].OutPtr]; - IOMapLowSpeed.OutBuf[ChannelNumber].OutPtr++; - } - if (dLowSpeedSendData(ChannelNumber, &VarsLowSpeed.OutputBuf[ChannelNumber].Buf[0], (VarsLowSpeed.OutputBuf[ChannelNumber].InPtr - VarsLowSpeed.OutputBuf[ChannelNumber].OutPtr))) + cLowSpeedLoadWriteBuffer(ChannelNumber); + if (dLowSpeedSendData(ChannelNumber, VarsLowSpeed.OutputBuf[ChannelNumber].Buf, VarsLowSpeed.OutputBuf[ChannelNumber].InPtr)) { if (IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx != 0) { @@ -167,16 +244,7 @@ void cLowSpeedCtrl(void) Temp = dLowSpeedComRxStatus(ChannelNumber); if (Temp == LOWSPEED_COMMUNICATION_SUCCESS) { - for (VarsLowSpeed.InputBuf[ChannelNumber].OutPtr = 0; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr < IOMapLowSpeed.InBuf[ChannelNumber].BytesToRx; VarsLowSpeed.InputBuf[ChannelNumber].OutPtr++) - { - IOMapLowSpeed.InBuf[ChannelNumber].Buf[IOMapLowSpeed.InBuf[ChannelNumber].InPtr] = VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr]; - IOMapLowSpeed.InBuf[ChannelNumber].InPtr++; - if (IOMapLowSpeed.InBuf[ChannelNumber].InPtr >= SIZE_OF_LSBUF) - { - IOMapLowSpeed.InBuf[ChannelNumber].InPtr = 0; - } - VarsLowSpeed.InputBuf[ChannelNumber].Buf[VarsLowSpeed.InputBuf[ChannelNumber].OutPtr] = 0; - } + cLowSpeedCompleteRead(ChannelNumber); IOMapLowSpeed.Mode[ChannelNumber] = LOWSPEED_DATA_RECEIVED; IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_DONE; } @@ -202,27 +270,14 @@ void cLowSpeedCtrl(void) case LOWSPEED_ERROR: { - IOMapLowSpeed.State = IOMapLowSpeed.State & ~LOWSPEED_CH_NUMBER[ChannelNumber]; - if (IOMapLowSpeed.State == 0) - { - dLowSpeedStopTimer(); - VarsLowSpeed.TimerState = TIMER_STOPPED; - } + cLowSpeedFinished(ChannelNumber, FALSE); } break; - case LOWSPEED_DONE: { - IOMapLowSpeed.State = IOMapLowSpeed.State & ~LOWSPEED_CH_NUMBER[ChannelNumber]; - IOMapLowSpeed.ChannelState[ChannelNumber] = LOWSPEED_IDLE; - if (IOMapLowSpeed.State == 0) - { - dLowSpeedStopTimer(); - VarsLowSpeed.TimerState = TIMER_STOPPED; - } + cLowSpeedFinished(ChannelNumber, TRUE); } break; - default: break; } diff --git a/AT91SAM7S256/Source/c_lowspeed.iom b/AT91SAM7S256/Source/c_lowspeed.iom index 15e0a8e..7fcec19 100644 --- a/AT91SAM7S256/Source/c_lowspeed.iom +++ b/AT91SAM7S256/Source/c_lowspeed.iom @@ -64,7 +64,12 @@ enum LOWSPEED_NO_ERROR = 0, LOWSPEED_CH_NOT_READY, LOWSPEED_TX_ERROR, - LOWSPEED_RX_ERROR + LOWSPEED_RX_ERROR, + LOWSPEED_FAST_INVALID_PORT = -1, + LOWSPEED_FAST_BUSY = -2, + LOWSPEED_FAST_FAULT = -3, + LOWSPEED_FAST_INVALID_LEN = -4, + LOWSPEED_FAST_BUS_BUSY = -5 }; // Constants referring to NoRestartOnRead @@ -79,7 +84,17 @@ enum COM_CHANNEL_NO_RESTART_MASK = 0x10 }; - +// Constants referring to Speed +enum +{ + COM_CHANNEL_NONE_FAST = 0x00, + COM_CHANNEL_ONE_FAST = 0x01, + COM_CHANNEL_TWO_FAST = 0x02, + COM_CHANNEL_THREE_FAST = 0x04, + COM_CHANNEL_FOUR_FAST = 0x08, + COM_CHANNEL_ALL_FAST = 0x0F, + COM_CHANNEL_SPEED_MASK = 0x10 +}; typedef struct { @@ -99,6 +114,8 @@ typedef struct UBYTE State; UBYTE Speed; UBYTE NoRestartOnRead; + UBYTE Spare1; + SBYTE (*pFunc)(UBYTE); }IOMAPLOWSPEED; diff --git a/AT91SAM7S256/Source/c_output.c b/AT91SAM7S256/Source/c_output.c index 9e7c2e0..cd826ca 100644 --- a/AT91SAM7S256/Source/c_output.c +++ b/AT91SAM7S256/Source/c_output.c @@ -47,10 +47,10 @@ void cOutputInit(void* pHeader) { OUTPUT * pOut = &(IOMapOutput.Outputs[Tmp]); pOut->Mode = 0x00; - pOut->Speed = 0x00; - pOut->ActualSpeed = 0x00; - pOut->TachoCnt = 0x00; - pOut->RunState = 0x00; + pOut->Speed = 0x00; + pOut->ActualSpeed = 0x00; + pOut->TachoCnt = 0x00; + pOut->RunState = 0x00; pOut->TachoLimit = 0x00; pOut->RegPParameter = DEFAULT_P_GAIN_FACTOR; pOut->RegIParameter = DEFAULT_I_GAIN_FACTOR; diff --git a/AT91SAM7S256/Source/d_input.c b/AT91SAM7S256/Source/d_input.c index 771eb3e..fcdb350 100644 --- a/AT91SAM7S256/Source/d_input.c +++ b/AT91SAM7S256/Source/d_input.c @@ -150,3 +150,17 @@ void dInputExit(void) INPUTExit; } +void dInputClockHigh(UBYTE Port) +{ + INPUTClkHigh(Port); +} + +void dInputClockLow(UBYTE Port) +{ + INPUTClkLow(Port); +} + +void dInputWaitUS(UBYTE usec) +{ + rInputWaitUS(usec); +} diff --git a/AT91SAM7S256/Source/d_input.h b/AT91SAM7S256/Source/d_input.h index d365dd1..98d23d8 100644 --- a/AT91SAM7S256/Source/d_input.h +++ b/AT91SAM7S256/Source/d_input.h @@ -44,5 +44,7 @@ void dInputSetColorClkInput(void); void dInputClearColor100msTimer(UBYTE No); UBYTE dInputChkColor100msTimer(UBYTE No); +void dInputWaitUS(UBYTE usec); + #endif diff --git a/AT91SAM7S256/Source/d_input.r b/AT91SAM7S256/Source/d_input.r index 3dc567e..c7226de 100644 --- a/AT91SAM7S256/Source/d_input.r +++ b/AT91SAM7S256/Source/d_input.r @@ -15,6 +15,8 @@ #ifdef SAM7S256 +void rInputWaitUS(UBYTE usec); + void rInputWait2uS(void); void rInputWait20uS(void); void rInputWait30uS(void); @@ -175,8 +177,6 @@ static ULONG ColorTimer[NO_OF_INPUTS]; CHECKColorState(Port, Status); \ } -#define SETClkHi(Port) INPUTClkHigh(Port) \ - #define COLORTx(Port, Data) { \ UBYTE BitCnt; \ BitCnt = 0; \ @@ -284,6 +284,14 @@ void GetAdVals(COLORSTRUCT *pColStruct, UBYTE Color, UBYTE Status) } } +void rInputWaitUS(UBYTE usec) +{ + // OSC = 48054850L + ULONG Count = (OSC/16)/(1000000L/usec); + ULONG PitTmr = (*AT91C_PITC_PIIR); + while (((*AT91C_PITC_PIIR) - PitTmr) < Count); +} + void rInputWait2uS(void) { ULONG PitTmr; diff --git a/AT91SAM7S256/Source/d_loader.h b/AT91SAM7S256/Source/d_loader.h index 9ccd01c..0914231 100644 --- a/AT91SAM7S256/Source/d_loader.h +++ b/AT91SAM7S256/Source/d_loader.h @@ -19,9 +19,9 @@ #define STARTOFFILETABLE (0x140000L - (FILETABLE_SIZE*4)) #define FILEPTRTABLE ((const ULONG*)(0x140000L - (FILETABLE_SIZE*4))) #ifndef STRIPPED -#define STARTOFUSERFLASH (0x125C00L)//(0x124600L) 1.31 == (0x122100L) +#define STARTOFUSERFLASH (0x126000L)//(0x124600L) 1.31 == (0x122100L) #else -#define STARTOFUSERFLASH (0x122400L)//(0x124600L) 1.31 == (0x122100L) +#define STARTOFUSERFLASH (0x122C00L)//(0x124600L) 1.31 == (0x122100L) #endif #define SIZEOFUSERFLASH ((ULONG)STARTOFFILETABLE - STARTOFUSERFLASH) diff --git a/AT91SAM7S256/Source/d_lowspeed.c b/AT91SAM7S256/Source/d_lowspeed.c index bb4f699..db272f5 100644 --- a/AT91SAM7S256/Source/d_lowspeed.c +++ b/AT91SAM7S256/Source/d_lowspeed.c @@ -15,6 +15,7 @@ #include "stdconst.h" #include "m_sched.h" #include "d_lowspeed.h" +#include #include "d_lowspeed.r" @@ -75,3 +76,8 @@ void dLowSpeedExit(void) { LOWSpeedExit; } + +SBYTE dLowSpeedFastI2C(UBYTE port, UBYTE address, UBYTE *write_data, UBYTE write_len, UBYTE *pReadLen, UBYTE *data_out) +{ + return rI2CFastStart(port, address, write_data, write_len, pReadLen, data_out); +} diff --git a/AT91SAM7S256/Source/d_lowspeed.h b/AT91SAM7S256/Source/d_lowspeed.h index 3c916c3..072e575 100644 --- a/AT91SAM7S256/Source/d_lowspeed.h +++ b/AT91SAM7S256/Source/d_lowspeed.h @@ -24,5 +24,6 @@ void dLowSpeedReceiveData(UBYTE ChannelNumber, UBYTE *DataInBuffer, UBYTE B UBYTE dLowSpeedComTxStatus(UBYTE ChannelNumber); UBYTE dLowSpeedComRxStatus(UBYTE ChannelNumber); void dLowSpeedExit(void); +SBYTE dLowSpeedFastI2C(UBYTE port, UBYTE address, UBYTE *write_data, UBYTE write_len, UBYTE *pReadLen, UBYTE *data_out); #endif diff --git a/AT91SAM7S256/Source/d_lowspeed.r b/AT91SAM7S256/Source/d_lowspeed.r index 93f0fb8..b82a208 100644 --- a/AT91SAM7S256/Source/d_lowspeed.r +++ b/AT91SAM7S256/Source/d_lowspeed.r @@ -711,7 +711,459 @@ __ramfunc void LowSpeedPwmIrqHandler(void) }\ } +typedef enum { + I2C_DISABLED = 0, + I2C_IDLE, + I2C_ACTIVEIDLE, + I2C_COMPLETE, + I2C_BEGIN, + I2C_NEWRESTART, + I2C_NEWSTART, + I2C_NEWREAD, + I2C_START1, + I2C_START2, + I2C_DELAY, + I2C_RXDATA1, + I2C_RXDATA2, + I2C_RXDATA3, + I2C_RXENDACK, + I2C_RXACK1, + I2C_RXACK2, + I2C_TXDATA1, + I2C_TXDATA2, + I2C_TXACK1, + I2C_TXACK2, + I2C_STOP1, + I2C_STOP2, + I2C_STOP3, + I2C_ENDLEGO1, + I2C_ENDLEGO2, + I2C_END, + I2C_ENDSTOP1, + I2C_ENDSTOP2, + I2C_ENDSTOP3, + I2C_FAULT, + I2C_RELEASE, +} i2c_port_state; + +struct i2c_partial_transaction { + i2c_port_state state; // Initial state for this transaction + UWORD nbits; // N bits to transfer + UBYTE * data; // Data buffer +}; + +#define I2C_ERR_INVALID_PORT -1 +#define I2C_ERR_BUSY -2 +#define I2C_ERR_FAULT -3 +#define I2C_ERR_INVALID_LENGTH -4 +#define I2C_ERR_BUS_BUSY -5 + +#define I2C_BUF_SIZE 16 +#define I2C_HS_CLOCK 125000 +#define I2C_MAX_PARTIAL_TRANSACTIONS 5 +// Fast pulse stretch +#define I2C_CLOCK_RETRY 5 +// Timeout for pulse stretch +#define I2C_MAX_STRETCH 100 +#define I2C_N_PORTS 4 + +#define TIME4US ((OSC/16)/250000L) + +#define IO_COMPLETE_MASK 0xf +#define BUS_FREE_MASK 0xf00 +#define BUS_FREE_SHIFT 8 + +// Take the clock line high but allow slave devices to extend the low clock +// state by pulling the clock line low. +static int rPulseStretch(ULONG scl_pin, SWORD * pDelay, i2c_port_state * pState) +{ + // Allow pulse stretching, make clock line float + *AT91C_PIOA_ODR = scl_pin; + // Has the pulse been stretched too much? If so report an error + *pDelay -= 1; + if (*pDelay <= 0) + { + *pState = I2C_FAULT; + return 1; + } + // Implement a fast wait routine, to avoid having to wait for an entire + // clock cycle. + int i = 0; + while(!(*AT91C_PIOA_PDSR & scl_pin)) + if (i++ >= I2C_CLOCK_RETRY) + return 1; + return 0; +} + +// Start a transaction. +SBYTE rI2CFastStart(UBYTE port, UBYTE address, UBYTE *write_data, UBYTE write_len, UBYTE *pReadLen, UBYTE *data_out) +{ + if (port >= I2C_N_PORTS) + return -1; + ULONG p_scl_pin; + ULONG p_sda_pin; + ULONG p_ready_mask; + UBYTE p_buffer[I2C_BUF_SIZE+2]; + struct i2c_partial_transaction p_partial_transaction[I2C_MAX_PARTIAL_TRANSACTIONS]; + struct i2c_partial_transaction *p_current_pt; + i2c_port_state p_state; + UBYTE *p_data; + ULONG p_nbits; + SWORD p_delay; + UBYTE p_bits; + UBYTE p_fault; + UBYTE p_lego_mode = FALSE; + UBYTE p_always_active = FALSE; + UBYTE p_no_release = FALSE; + UBYTE p_high_speed = TRUE; + + ULONG pinmask; + + // enable the port in hi speed mode + { + p_scl_pin = CLK_PINS[port]; + p_sda_pin = DATA_PINS[port]; + pinmask = CLK_OR_DATA_PINS[port]; + p_state = I2C_IDLE; + // Set data & clock to be enabled for output with + // pullups disabled. + + *AT91C_PIOA_SODR = pinmask; + *AT91C_PIOA_OER = pinmask; + *AT91C_PIOA_PPUDR = pinmask; +// // If we are always active, we never drop below the ACTIVEIDLE state +// p_lego_mode = ((mode & I2C_LEGO_MODE) ? 1 : 0); +// p_no_release = ((mode & I2C_NO_RELEASE) ? 1 : 0); +// p_always_active = ((mode & I2C_ALWAYS_ACTIVE) ? 1 : 0); + if (p_always_active) + p_state = I2C_ACTIVEIDLE; + + // Select which lines to test to see if the bus is busy. + // If the clock line is being driven by us we do not test it. + if (p_lego_mode || p_no_release) + p_ready_mask = p_sda_pin; + else + p_ready_mask = pinmask; + // Release whichever lines we can + *AT91C_PIOA_ODR = p_ready_mask; + } + + struct i2c_partial_transaction *pt; + UBYTE *data; + + // check buffer size + if (*pReadLen > I2C_BUF_SIZE) return I2C_ERR_INVALID_LENGTH; + if (write_len > I2C_BUF_SIZE) return I2C_ERR_INVALID_LENGTH; + // must have some data to transfer + if (*pReadLen + write_len <= 0) return I2C_ERR_INVALID_LENGTH; + + pt = p_partial_transaction; + p_current_pt = pt; + data = p_buffer; + + // process the write data (if any) + if (write_len > 0){ + *data++ = address; // This is a write + pt->nbits = (write_len + 1)*8; + // copy the write data + memcpy(data, write_data, write_len); + data += write_len; + pt->data = p_buffer; + pt->state = I2C_NEWSTART; + // We add an extra stop for the odd Lego i2c sensor, but only on a read + if (*pReadLen > 0 && p_lego_mode) { + pt++; + pt->state = I2C_STOP1; + } + pt++; + } + // now add the read transaction (if any) + if (*pReadLen > 0) + { + // first we have to write the device address + pt->state = (data != p_buffer ? I2C_NEWRESTART : I2C_NEWSTART); + pt->data = data; + *data++ = address | 1; // this is a read + pt->nbits = 8; + pt++; + // now we have the read + pt->state = I2C_NEWREAD; + pt->data = p_buffer; + pt->nbits = (*pReadLen)*8; + pt++; + } + // define what happens at the end of the operation + if (p_lego_mode) + pt->state = (*pReadLen > 0 ? I2C_ENDLEGO1 : I2C_ENDSTOP1); + else + pt->state = (p_no_release ? I2C_END : I2C_ENDSTOP1); +// // We save the number of bytes to read for completion +// p->read_len = read_len; + // Start the transaction + p_state = I2C_BEGIN; +// if (!p_always_active) +// build_active_list(); + while(p_state != I2C_COMPLETE) + { + ULONG PitTmr = (*AT91C_PITC_PIIR); + // The main i2c state machine. Move the port state from one step to another + // toggling the clock line on alternate calls. + switch (p_state) { + default: + case I2C_DISABLED: + case I2C_IDLE: // Not in a transaction + case I2C_ACTIVEIDLE: // Not in a transaction but active + case I2C_COMPLETE: // Transaction completed + break; + case I2C_BEGIN: + // Start new transaction + *AT91C_PIOA_OER = p_sda_pin|p_scl_pin; + p_fault = 0; + p_state = p_current_pt->state; + break; + case I2C_NEWRESTART: + // restart a new partial transaction + // Take the clock low + *AT91C_PIOA_CODR = p_scl_pin; + // FALLTHROUGH + case I2C_NEWSTART: + // Start the current partial transaction + p_data = p_current_pt->data; + p_nbits = p_current_pt->nbits; + p_bits = *(p_data); + p_state = I2C_START1; + p_delay = I2C_MAX_STRETCH; + *AT91C_PIOA_SODR = p_sda_pin; + *AT91C_PIOA_OER = p_sda_pin; + break; + case I2C_START1: + // SDA high, take SCL high + if (p_high_speed && rPulseStretch(p_scl_pin, &p_delay, &p_state)) + break; + *AT91C_PIOA_SODR = p_scl_pin; + *AT91C_PIOA_OER = p_scl_pin; + p_state = I2C_START2; + break; + case I2C_START2: + // Take SDA low while SCL is high + *AT91C_PIOA_CODR = p_sda_pin; + p_state = I2C_TXDATA1; + break; + case I2C_DELAY: + if (p_delay == 0) + p_state = I2C_RXDATA1; + else + p_delay--; + break; + case I2C_TXDATA1: + // Take SCL low + *AT91C_PIOA_CODR = p_scl_pin; + p_delay = I2C_MAX_STRETCH; + p_state = I2C_TXDATA2; + break; + case I2C_TXDATA2: + // set the data line + if (p_bits & 0x80) + *AT91C_PIOA_SODR = p_sda_pin; + else + *AT91C_PIOA_CODR = p_sda_pin; + *AT91C_PIOA_OER = p_sda_pin; + if (p_high_speed && rPulseStretch(p_scl_pin, &p_delay, &p_state)) + break; + p_bits <<= 1; + p_nbits--; + if((p_nbits & 7) == 0) + p_state = I2C_TXACK1; + else + p_state = I2C_TXDATA1; + // Take SCL high + *AT91C_PIOA_SODR = p_scl_pin; + *AT91C_PIOA_OER = p_scl_pin; + break; + case I2C_TXACK1: + // Take SCL low + *AT91C_PIOA_CODR = p_scl_pin; + p_state = I2C_TXACK2; + // release the data line + *AT91C_PIOA_ODR = p_sda_pin; + break; + case I2C_TXACK2: + // Take SCL High + *AT91C_PIOA_SODR = p_scl_pin; + if(*AT91C_PIOA_PDSR & p_sda_pin) + p_state = I2C_FAULT; + else if (p_nbits == 0) + { + p_current_pt++; + p_state = p_current_pt->state; + } + else + { + p_data++; + p_bits = *(p_data); + p_state = I2C_TXDATA1; + } + break; + case I2C_NEWREAD: + // Start the read partial transaction + p_data = p_current_pt->data; + p_nbits = p_current_pt->nbits; + p_bits = 0; + // Take SCL Low + *AT91C_PIOA_CODR = p_scl_pin; + p_state = I2C_RXDATA1; + // get ready to read + *AT91C_PIOA_ODR = p_sda_pin; + break; + case I2C_RXDATA1: + p_delay = I2C_MAX_STRETCH; + p_state = I2C_RXDATA2; + // Fall through + case I2C_RXDATA2: + if (p_high_speed && rPulseStretch(p_scl_pin, &p_delay, &p_state)) + break; + // Take SCL High + *AT91C_PIOA_SODR = p_scl_pin; + *AT91C_PIOA_OER = p_scl_pin; + p_state = I2C_RXDATA3; + break; + case I2C_RXDATA3: + // Receive a bit. + p_bits <<= 1; + if(*AT91C_PIOA_PDSR & p_sda_pin) + p_bits |= 1; + // Take SCL Low + *AT91C_PIOA_CODR = p_scl_pin; + p_nbits--; + if((p_nbits & 7) == 0){ + *(p_data) = p_bits; + if (p_nbits) + p_state = I2C_RXACK1; + else + p_state = I2C_RXENDACK; + } + else + p_state = I2C_RXDATA1; + break; + case I2C_RXACK1: + // take data low + *AT91C_PIOA_CODR = p_sda_pin; + *AT91C_PIOA_OER = p_sda_pin; + // Move on to next byte + p_data++; + p_bits = 0; + p_state = I2C_RXACK2; + // Clock high + *AT91C_PIOA_SODR = p_scl_pin; + break; + case I2C_RXACK2: + // Take SCL Low + *AT91C_PIOA_CODR = p_scl_pin; + p_state = I2C_RXDATA1; + // get ready to read + *AT91C_PIOA_ODR = p_sda_pin; + break; + case I2C_RXENDACK: + // take data high + *AT91C_PIOA_SODR = p_sda_pin; + *AT91C_PIOA_OER = p_sda_pin; + // get ready to move to the next state + p_current_pt++; + p_state = p_current_pt->state; + // Clock high data is already high + *AT91C_PIOA_SODR = p_scl_pin; + break; + case I2C_STOP1: + // Issue a Stop state + // SCL is high, take it low + *AT91C_PIOA_CODR = p_scl_pin; + p_state = I2C_STOP2; + *AT91C_PIOA_CODR = p_sda_pin; + *AT91C_PIOA_OER = p_sda_pin; + break; + case I2C_STOP2: + // Take SCL high + *AT91C_PIOA_SODR = p_scl_pin; + p_state = I2C_STOP3; + break; + case I2C_STOP3: + // Take SDA pin high while the clock is high + *AT91C_PIOA_SODR = p_sda_pin; + // and move to the next state + p_current_pt++; + p_state = p_current_pt->state; + break; + case I2C_ENDLEGO1: + // Lego mode end case, does not issue stop. Used at end of rx + // Clock low data is already high, release the data line + *AT91C_PIOA_CODR = p_scl_pin; + p_state = I2C_ENDLEGO2; + *AT91C_PIOA_ODR = p_sda_pin; + break; + case I2C_ENDLEGO2: + // Clock high, data is already high and we are done + *AT91C_PIOA_SODR = p_scl_pin; + p_state = I2C_RELEASE; + break; + case I2C_END: + // End the transaction but hold onto the bus, keeping the clock low + // Clock low and keep it active + *AT91C_PIOA_CODR = p_scl_pin; + p_state = I2C_RELEASE; + break; + case I2C_ENDSTOP1: + // Issue a Stop state + // SCL is high, take it low + *AT91C_PIOA_CODR = p_scl_pin; + p_delay = I2C_MAX_STRETCH; + p_state = I2C_ENDSTOP2; + *AT91C_PIOA_CODR = p_sda_pin; + *AT91C_PIOA_OER = p_sda_pin; + break; + case I2C_ENDSTOP2: + if (p_high_speed && rPulseStretch(p_scl_pin, &p_delay, &p_state)) + break; + // Take SCL high + *AT91C_PIOA_SODR = p_scl_pin; + *AT91C_PIOA_OER = p_scl_pin; + p_state = I2C_ENDSTOP3; + break; + case I2C_ENDSTOP3: + // Take SDA pin high while the clock is high + *AT91C_PIOA_SODR = p_sda_pin; + p_state = I2C_RELEASE; + break; + case I2C_FAULT: + p_fault = 1; + p_state = I2C_ENDSTOP1; + break; + case I2C_RELEASE: + // Release whichever lines we can + *AT91C_PIOA_ODR = p_ready_mask; + // All done + p_state = I2C_COMPLETE; + break; + } + while (((*AT91C_PITC_PIIR) - PitTmr) < TIME4US); + } + + if (!p_always_active) + p_state = I2C_IDLE; + else + p_state = I2C_ACTIVEIDLE; + if (p_fault) + return I2C_ERR_FAULT; + if (*pReadLen > 0) + { + *pReadLen = (UBYTE)(p_data - p_buffer + 1); + if (data_out) + memcpy(data_out, p_buffer, *pReadLen); + } + return *pReadLen; + +} + #endif #ifdef PCWIN -- cgit v1.2.3