int cog_endStackTest(int *coginfo)
{
int cog = *coginfo - 1;
int *addr = coginfo;
int stacksize = 0;
//print("\n\n---[ cog_end ]---\n\n");
//print("Cog Address = %d, Cog Value = %d\n", (int) addr, *addr);
//print("Stack Count Address = %d, Stack Count = %d\n\n", (int) (addr+1), *(addr+1));
int stackInts = *(addr+1);
int stackOverhead = sizeof(_thread_state_t) + (3 * sizeof(unsigned int));
//print("stackOverhead = %d bytes, %d ints\n", stackOverhead, stackOverhead/sizeof(int));
int cogRunTestOverhead = 2 * sizeof(int);
//print("cogRunTestOverhead = %d bytes, %d ints\n", cogRunTestOverhead, cogRunTestOverhead/sizeof(int));
int stackOther = stackInts - (stackOverhead/sizeof(int)) - (cogRunTestOverhead/sizeof(int));
//print("stackOther = %d ints\n\n", stackOther);
srand(stackOther);
//srand(0);
int n = -2;
for(int *i = addr; i < (addr + (stackInts)); i++)
{
//print("idx = %d, addr = %d, val = %d\n", (int)(i-addr-2), (int) i, *i);
if((n >= 0) && (stacksize == 0))
{
//if(*i != n)
if(*i != rand())
{
stacksize = stackOther - n;
}
}
n++;
}
//print("stacksize = %d ints\n", stacksize);
if(cog > -1)
{
if(cog == cogid())
{
free(coginfo);
cogstop(cog);
}
else
{
cogstop(cog);
free(coginfo);
}
}
*coginfo = 0;
if(stacksize < 0) stacksize = 0;
return stacksize;
}
void gobkwd(void *par)
{
running = 1;
drive_goto(-256, -256);
running = 0;
cogstop(cogid());
}
void gofwd(void *par)
{
running = 1;
drive_goto(256, 256);
running = 0;
cogstop(cogid());
}
/**
* stop stops the cog running the native assembly driver
*/
void FdSerial_stop(FdSerial_t *data)
{
if(data->cogId > 0) {
cogstop(data->cogId - 1);
data->cogId = 0;
}
}
void RC::Stop(void)
{
if(Cog) {
cogstop(Cog - 1);
Cog = 0;
}
}
/**
* stop stops the cog running the native assembly driver
*/
void tvText_stop(void)
{
int id = gTvTextCog - 1;
if(gTvTextCog > 0) {
cogstop(id);
}
}
void F32::Stop(void)
{
// Stop floating point engine and release the cog.
if(cog) {
cogstop(cog - 1);
cog = 0;
}
}
void mstime_stop()
{
if(cog)
{
cogstop(cog -1);
cog = 0;
}
}
void SBUS::Stop(void)
{
// Stop driver and release cog
if(Cog) {
cogstop(Cog - 1);
Cog = 0;
}
}
void square_wave_stop(void)
{
if(cog)
{
cogstop(cog - 1);
cog = 0;
}
}
int32_t ILI9341_spi::Stop(void)
{
if (cog != 0) {
cogstop((cog - 1));
}
cog = 0;
return 0;
}
I2C::~I2C()
{
if (m_par.cog >= 0)
{
cogstop(m_par.cog);
m_par.cog = -1;
m_ready = 0;
}
}
void servo_stop(void) // Stop servo process, free a cog
{
if(servoCog) // If the cog is running
{
cogstop(servoCog-1); // Stop it
servoCog = 0; // Remember that it's stopped
lockret(lockID); // Return the lock
}
}
void cog_end(int *coginfo)
{
int cog = *coginfo - 1;
if(cog > -1)
{
if(cog == cogid())
{
free(coginfo);
cogstop(cog);
}
else
{
cogstop(cog);
free(coginfo);
}
}
*coginfo = 0;
}
void sound_end(sound_t *device)
{
if(device->cog > 0)
{
cogstop(device->cog - 1);
device->cog = 0;
free(device);
}
}
uint8_t SPIStop (void) {
if (!SPIIsRunning())
return 0;
cogstop(g_spiCog);
g_spiCog = -1;
g_mailbox = -1;
return 0;
}
/* start cogA */
int start_cogA(volatile void *parptr)
{
extern unsigned int _load_start_cogA_cog[]; //start address for cog code
extern unsigned int _load_stop_cogA_cog[]; //end address for cog code
if(parA.A.cog != -1) //see if the cog is running
cogstop(parA.A.cog); //stop the cog
int size = (_load_stop_cogA_cog - _load_start_cogA_cog)*4; //code size in bytes
printf(" cogA code size %i bytes - ",size);
unsigned int code[size]; //allocate enough HUB to hold the COG code
memcpy(code, _load_start_cogA_cog, size); //copy the code to HUB memory
parA.A.cog = cognew(code, parptr); //start the cog
return parA.A.cog;
}
void light_stop(void)
{
// Stops charlieplex cog if running
if (self->cog) {
// running?
// yes, stop it
cogstop((self->cog - 1));
// mark stopped
self->cog = 0;
}
// clear led bits (for re-start)
self->ledbits = 0;
}
int gps_close()
{
if(cog >= 0)
{
memset(&gps_data, 0, sizeof(nmea_data)); //clear the GPS data structure
stopping = 1;
while(stopping)
pause(1);
cogstop(cog);
cog = -1;
}
}
/* Releases a pin from this servo object and resets the slot state.
*/
void Servo::detach()
{
// Set lock.
while(lockset(Servo::_lockID));
Servo::_slots[_idx]._pin = -1;
Servo::_slots[_idx]._currentPulse = DEFAULT_PULSE_WIDTH;
Servo::_slots[_idx]._previousPulse = DEFAULT_PULSE_WIDTH;
Servo::_slots[_idx]._stepSize = 2000;
// Decrement the attached servo count.
Servo::_attachedCount--;
// if no more attached servos and the cog is running,
// then stop the cog and record that it's stopped.
if (Servo::_attachedCount == 0 && Servo::_servoCog != 0)
{
cogstop(Servo::_servoCog-1);
Servo::_servoCog = 0;
}
// Clear lock
lockclr(Servo::_lockID);
}
int main(void)
{
uint8_t *buffer = (uint8_t *)(((uint32_t)padded_buffer + 15) & ~15);
SdLoaderInfo *info = (SdLoaderInfo *)_load_start_coguser0;
uint32_t load_address, index_width, offset_width, addr, count;
uint32_t tags_size, cache_size, cache_lines, cache_tags, mboxes, *sp;
VolumeInfo vinfo;
FileInfo finfo;
PexeFileHdr *hdr;
int sd_id, i;
uint8_t *p;
// determine the cache size and setup cache variables
index_width = info->cache_geometry >> 8;
offset_width = info->cache_geometry & 0xff;
tags_size = (1 << index_width) * 4;
cache_size = 1 << (index_width + offset_width);
cache_lines = HUB_SIZE - cache_size;
cache_tags = cache_lines - tags_size;
mboxes = cache_tags - sizeof(xmem_mbox_t) * 8 - 4;
xmem_mbox = (xmem_mbox_t *)mboxes;
// load the external memory driver
DPRINTF("Loading external memory driver\n");
memset((void *)mboxes, 0, sizeof(xmem_mbox_t) * 8 + 4);
xmem_mbox[8].hubaddr = XMEM_END;
cognew(_load_start_coguser1, mboxes);
DPRINTF("Loading SD driver\n");
sd_mbox = (uint32_t *)mboxes - 2;
sd_mbox[0] = 0xffffffff;
if ((sd_id = cognew(_load_start_coguser2, sd_mbox)) < 0) {
DPRINTF("Error loading SD driver\n");
return 1;
}
while (sd_mbox[0])
;
DPRINTF("Initializing SD card\n");
if (SD_Init(sd_mbox, 5) != 0) {
DPRINTF("SD card initialization failed\n");
return 1;
}
DPRINTF("Mounting filesystem\n");
if (MountFS(buffer, &vinfo) != 0) {
DPRINTF("MountFS failed\n");
return 1;
}
// open the .pex file
DPRINTF("Opening 'autorun.pex'\n");
if (FindFile(buffer, &vinfo, FILENAME, &finfo) != 0) {
DPRINTF("FindFile '%s' failed\n", FILENAME);
return 1;
}
// read the file header
DPRINTF("Reading PEX file header\n");
if (GetNextFileSector(&finfo, kernel_image, &count) != 0 || count < PEXE_HDR_SIZE) {
DPRINTF("Error reading PEX file header\n");
return 1;
}
// verify the header
DPRINTF("Verifying PEX file header\n");
hdr = (PexeFileHdr *)kernel_image;
if (strncmp(hdr->tag, PEXE_TAG, sizeof(hdr->tag)) != 0 || hdr->version != PEXE_VERSION) {
DPRINTF("Bad PEX file header\n");
return 1;
}
load_address = hdr->loadAddress;
// move past the header
memmove(kernel_image, (uint8_t *)kernel_image + PEXE_HDR_SIZE, SECTOR_SIZE - PEXE_HDR_SIZE);
p = (uint8_t *)kernel_image + SECTOR_SIZE - PEXE_HDR_SIZE;
// read the .kernel cog image
DPRINTF("Reading kernel image\n");
for (i = 1; i < 4; ++i) {
if (GetNextFileSector(&finfo, p, &count) != 0 || count < SECTOR_SIZE)
return 1;
p += SECTOR_SIZE;
}
// load the image
DPRINTF("Loading image at 0x%08x\n", load_address);
addr = load_address;
while (GetNextFileSector(&finfo, buffer, &count) == 0) {
write_block(addr, buffer, XMEM_SIZE_512);
addr += SECTOR_SIZE;
}
// stop the sd driver
DPRINTF("Stopping SD driver\n");
cogstop(sd_id);
// setup the stack
// at start stack contains xmem_mbox, cache_tags, cache_lines, cache_geometry, pc
sp = (uint32_t *)mboxes;
*--sp = load_address;
*--sp = info->cache_geometry;
*--sp = cache_lines;
*--sp = cache_tags;
*--sp = mboxes;
// start the xmm kernel boot code
DPRINTF("Starting kernel\n");
coginit(cogid(), kernel_image, sp);
// should never reach this
return 0;
}
void dac_ctr_stop(void)
{
if(cog) cogstop(cog - 1);
cog = 0;
}
void pwm_stop(void)
{
if(pwcog) cogstop(pwcog - 1);
pwcog = 0;
}
/****************************** start main routine ******************************/
int main(void)
{
char input_buffer[INPUT_BUFFER]; //buffer for user input
waitcnt(500000 + _CNT); //wait until initialization is complete
printf("XMMC-cogc demo v%s",VERSION); //display startup message
/* start monitoring the real time clock DS3231 */
int cog;
rtc_cb.rtc.tdb_lock = locknew();
lockclr(rtc_cb.rtc.tdb_lock);
cog = start_rtc(&rtc_cb.rtc);
if(cog == -1)
{
printf("** error attempting to start rtc cog\n cognew returned %i\n\n",cog);
return 1;
}
printf(" DS3231 monitored by code running on cog %i\n",cog);
/* set all cogs to not running */
parA.A.cog = -1;
parB.B.cog = -1;
parC.C.cog = -1;
/* loop forever */
while(1)
{
printf("\nenter command > "); //prompt user
fgets(input_buffer,INPUT_BUFFER,stdin); //get a line of input
input_buffer[strlen(input_buffer)-1] = '\0'; //get rid of trailing new line character
switch(process(input_buffer)) //test input,take appropriate action
{
case 0: //startA
if(start_cogA(&parA.A)== -1)
printf(" problem starting cogA\n");
else
printf(" cogA started\n");
break;
case 1: //startB
if(start_cogB(&parB.B)== -1)
printf(" problem starting cogB\n");
else
printf(" cogB started\n");
break;
case 2: //startC
if(start_cogC(&parC.C)== -1)
printf(" problem starting cogC\n");
else
printf(" cogC started\n");
break;
case 3: //stopA
cogstop(parA.A.cog);
printf(" cog A stopped\n");
parA.A.cog = -1;
break;
case 4: //stopB
cogstop(parB.B.cog);
printf(" cog B stopped\n");
parB.B.cog = -1;
break;
case 5: //stopC
cogstop(parC.C.cog);
printf(" cog C stopped\n");
parC.C.cog = -1;
break;
case 6: //queryA
if(parA.A.cog == -1)
printf("cog A is not running\n");
else
parA.A.query_flag = 1;
break;
case 7: //queryB
if(parB.B.cog == -1)
printf("cog B is not running\n");
else
parB.B.query_flag = 1;
break;
case 8: //queryC
if(parC.C.cog == -1)
printf("cog C is not running\n");
else
parC.C.query_flag = 1;
break;
case 9: //status
status();
break;
case 10: //exit
printf("exiting program\n");
return 0;
case 11: //time
printf("%s, %i:%02i:%02i %i/%i/%i\n\n",
day_names_long[rtc_cb.rtc.dow-1],
rtc_cb.rtc.hour,
rtc_cb.rtc.min,
rtc_cb.rtc.sec,
rtc_cb.rtc.month,
rtc_cb.rtc.day,
rtc_cb.rtc.year+2000);
break;
default:
printf("<%s> is not a valid command\n",input_buffer);
}
}
return 0;
}
static int cog_i2cClose(I2C *dev)
{
I2C_COGDRIVER *cdev = (I2C_COGDRIVER *)dev;
cogstop(cdev->cog);
return 0;
}
