bool FirewirePort::WriteQuadlet(unsigned char boardId, nodeaddr_t addr, quadlet_t data)
{
int node = GetNodeId(boardId);
if (node < MAX_NODES)
return !raw1394_write(handle, baseNodeId+node, addr, 4, &data);
else
return false;
}
bool FirewirePort::WriteBlock(unsigned char boardId, nodeaddr_t addr, quadlet_t *data,
unsigned int nbytes)
{
int node = GetNodeId(boardId);
if (node < MAX_NODES)
return !raw1394_write(handle, baseNodeId+node, addr, nbytes, data);
else
return false;
}
int write_data(raw1394handle_t h, nodeid_t node, nodeaddr_t address, size_t length, quadlet_t *buf)
{
int ret;
while((ret = raw1394_write(h ,node, address, length, buf)) && errno == EAGAIN)
usleep(100000); // wait 100ms, device is not ready, since this is part of the initialisation it is no problem (from a real-time perspective) to retry
if(ret)
fprintf(stderr, "Failed to write data: (%d) %s\n", errno, strerror(errno));
return ret;
}
int write_data_char(raw1394handle_t h, nodeid_t node, nodeaddr_t address, unsigned char buf)
{
int ret;
while((ret = raw1394_write(h, node, address, 1, (quadlet_t *) &buf)) && errno == EAGAIN)
{
printf("waiting to write data\n");
usleep(100000); // wait 100ms, device is not ready, since this is part of the initialisation it is no problem (from a real-time perspective) to retry
}
if(ret)
fprintf(stderr, "Failed to write char: (%d) %s\n", errno, strerror(errno));
return ret;
}
int cooked1394_write(raw1394handle_t handle, nodeid_t node, nodeaddr_t addr,
size_t length, quadlet_t *data)
{
int retval, i;
struct timespec ts = {0, RETRY_DELAY};
for(i=0; i<MAXTRIES; i++) {
retval = raw1394_write(handle, node, addr, length, data);
if (retval < 0 && errno == EAGAIN)
nanosleep(&ts, NULL);
else
return retval;
}
return -1;
}
void FirewirePort::StopCycleStartPacket(void)
{
// IMPORTANT: Disable Cycle Start Packet, no isochronous
int rc = 0; // return code
quadlet_t data_stop_cmc = bswap_32(0x100);
rc = raw1394_write(handle,
raw1394_get_local_id(handle),
CSR_REGISTER_BASE + CSR_STATE_CLEAR,
4,
&data_stop_cmc);
if (rc) {
outStr << "*****Error: can NOT disable cycle start packet" << std::endl;
} else {
outStr << "FirewirePort: successfully disabled cycle start packet" << std::endl;
}
}
/** Try again upto MAX_RETRIES if raw1394_write returns EGAIN. */
int
raw1394_write_retry(raw1394handle_t handle, nodeid_t node, nodeaddr_t addr,
size_t length, quadlet_t *buffer)
{
int retries = MAX_RETRIES;
int ret;
again:
ret = raw1394_write(handle, node, addr, length, buffer);
if (ret != 0) {
if ((errno == EAGAIN) && (retries-- > 0))
/* Seems to be a bug */
goto again;
}
return ret;
}
bool FirewirePort::WriteAllBoardsBroadcast(void)
{
// check hanle
if (!handle) {
outStr << "WriteAllBoardsBroadcast: handle for port " << PortNum << " is NULL" << std::endl;
return false;
}
// sanity check vars
bool allOK = true;
bool noneWritten = true;
// loop 1: broadcast write block
// construct broadcast write buffer
const int numOfChannel = 4;
quadlet_t bcBuffer[numOfChannel * MAX_NODES];
memset(bcBuffer, 0, sizeof(bcBuffer));
int bcBufferOffset = 0; // the offset for new data to be stored in bcBuffer (bytes)
int numOfBoards = 0;
for (int board = 0; board < max_board; board++) {
if (BoardList[board]) {
numOfBoards++;
quadlet_t *buf = BoardList[board]->GetWriteBuffer();
unsigned int numBytes = BoardList[board]->GetWriteNumBytes();
memcpy(bcBuffer + bcBufferOffset/4, buf, numBytes-4); // -4 for ctrl offset
// bcBufferOffset equals total numBytes to write, when the loop ends
bcBufferOffset = bcBufferOffset + numBytes - 4;
}
}
// now broadcast out the huge packet
bool ret = true;
#if FAKEBC
ret = !raw1394_write(handle,
baseNodeId,
0xffffffff0000,
bcBufferOffset,
bcBuffer);
#else
ret = WriteBlockBroadcast(0xffffff000000, // now the address is hardcoded
bcBuffer,
bcBufferOffset);
#endif
// loop 2: send out control quadlet if necessary
for (int board = 0; board < max_board; board++) {
if (BoardList[board]) {
quadlet_t *buf = BoardList[board]->GetWriteBuffer();
unsigned int numBytes = BoardList[board]->GetWriteNumBytes();
unsigned int numQuads = numBytes/4;
quadlet_t ctrl = buf[numQuads-1]; // Get last quedlet
bool ret2 = true;
if (ctrl) { // if anything non-zero, write it
ret2 = WriteQuadlet(board, 0x00, ctrl);
if (ret2) noneWritten = false;
else allOK = false;
}
// SetWriteValid clears the buffer if the write was valid
BoardList[board]->SetWriteValid(ret&&ret2);
}
}
// pullEvents
if (noneWritten) {
PollEvents();
}
// return
return allOK;
}
bool FirewirePort::ReadAllBoardsBroadcast(void)
{
if (!handle || !handle_bc) {
outStr << "ReadAllBoardsBroadcast: handle for port " << PortNum << " is NULL" << std::endl;
return false;
}
bool ret;
bool allOK = true;
bool noneRead = true;
int hub_node_id = GetNodeId(HubBoard_->BoardId); // ZC: NOT USE PLACEHOLDER
//--- send out broadcast read request -----
#if 0
quadlet_t debugData;
nodeaddr_t debugAddr = 0x03;
bool retdebug = !raw1394_read(handle,
baseNodeId + hub_node_id, // boardid 7
debugAddr, // read from hub addr
4, // read all 16 boards
&debugData);
if (!retdebug) {
raw1394_errcode_t ecode = raw1394_get_errcode(handle);
std::cerr << "debug read ecode = " << ecode << " to_errno = " << raw1394_errcode_to_errno(ecode) << " "
<< strerror(raw1394_errcode_to_errno(ecode)) << std::endl;
}
#endif
// sequence number from 16 bits 0 to 65535
ReadSequence_++;
if (ReadSequence_ == 65536) {
ReadSequence_ = 1;
}
quadlet_t bcReqData = bswap_32((ReadSequence_ << 16) + BoardExistMask_);
nodeaddr_t bcReqAddr = 0xffffffff000F; // special address to trigger broadcast read
#if FAKEBC
ret = !raw1394_write(handle,
baseNodeId,
bcReqAddr,
4,
&bcReqData);
if (!ret) {
raw1394_errcode_t ecode = raw1394_get_errcode(handle);
std::cerr << "bbbbbbb fake ecode = " << ecode << " to_errno = " << raw1394_errcode_to_errno(ecode) << " "
<< strerror(raw1394_errcode_to_errno(ecode)) << std::endl;
}
#else
WriteQuadletBroadcast(bcReqAddr, bcReqData);
#endif
// // Manual sleep 50 us
timeval start, check;
gettimeofday(&start, NULL);
while(true) {
gettimeofday(&check, NULL);
if (((check.tv_sec-start.tv_sec)*1000000 + check.tv_usec-start.tv_usec) > (5.0*NumOfNodes_+10.0)) {
break;
}
}
// initialize max buffer
const int hubReadSize = 272; // 16 * 17 = 272 max
quadlet_t hubReadBuffer[hubReadSize];
memset(hubReadBuffer, 0, sizeof(hubReadBuffer));
// raw1394_read 0 = SUCCESS, -1 = FAIL, flip return value
ret = !raw1394_read(handle,
baseNodeId + hub_node_id,
0x1000, // read from hub addr
272 * 4, // read all 16 boards
hubReadBuffer);
// ----- DEBUG -----------
static int raw1394readCounter = 0;
if (!ret) {
raw1394readCounter++;
raw1394_errcode_t ecode = raw1394_get_errcode(handle);
std::cerr << "ecode = " << ecode << " to_errno = " << raw1394_errcode_to_errno(ecode) << " "
<< strerror(raw1394_errcode_to_errno(ecode)) << std::endl;
std::cerr << "raw1394_read failed " << raw1394readCounter << ": " << strerror(errno) << std::endl;
}
// -----------------------
for (int board = 0; board < max_board; board++) {
if (BoardList[board]) {
const int readSize = 17; // 1 seq + 16 data, unit quadlet
quadlet_t readBuffer[readSize];
memcpy(readBuffer, &(hubReadBuffer[readSize * board + 0]), readSize * 4);
unsigned int seq = (bswap_32(readBuffer[0]) >> 16);
static int errorcounter = 0;
if (ReadSequence_ != seq) {
errorcounter++;
outStr << "errorcounter = " << errorcounter << std::endl;
outStr << std::hex << seq << " " << ReadSequence_ << " " << (int)board << std::endl;
}
memcpy(BoardList[board]->GetReadBuffer(), &(readBuffer[1]), (readSize-1) * 4);
if (ret) noneRead = false;
else allOK = false;
BoardList[board]->SetReadValid(ret);
}
}
if (noneRead) {
PollEvents();
}
return allOK;
}
int
main(int argc, char **argv)
{
/* Command line parsing */
int opt;
unsigned port = 0;
unsigned step = 128;
enum { INVALID, PEEK, POKE, SCREEN } mode = INVALID;
const char *name = strippath(argv[0]);
if (strcmp(name, "fw_peek") == 0) {
mode = PEEK;
} else if (strcmp(name, "fw_poke") == 0) {
mode = POKE;
#ifndef NO_FW_SCREEN
} else if (strcmp(name, "fw_screen") == 0) {
mode = SCREEN;
#endif
}
if (mode == INVALID) {
fprintf(stderr, "Could not decide, whether we are fw_peek or fw_poke.\n");
return EXIT_FAILURE;
}
while ((opt = getopt(argc, argv, "p:b:")) != -1) {
switch (opt) {
case 'p':
port = strtoul(optarg, 0, 0);
break;
case 'b':
step = strtoul(optarg, 0, 0);
break;
default:
goto print_usage;
}
}
if (((mode == PEEK) && (argc - optind) != 3) ||
((mode == POKE) && (argc - optind) != 2) ||
((mode == SCREEN) && (argc - optind) != 5)) {
print_usage:
fprintf(stderr, (mode == PEEK) ? usage_peek :
(mode == POKE) ? usage_poke : usage_screen, name);
return EXIT_FAILURE;
}
uint64_t guid = strtoull(argv[optind], NULL, 0);
uint64_t address = strtoull(argv[optind + 1], NULL, 0);
uint64_t length;
uint32_t width, height, depth;
if (mode == PEEK) length = strtoull(argv[optind + 2], NULL, 0);
if (mode == SCREEN) {
width = strtoul(argv[optind + 2], NULL, 0);
height = strtoul(argv[optind + 3], NULL, 0);
depth = strtoul(argv[optind + 4], NULL, 0);
length = 1ULL * depth / 8 * width * height;
}
raw1394handle_t fw_handle = raw1394_new_handle_on_port(port);
if (fw_handle == NULL) {
perror("raw1394_new_handle_on_port");
return EXIT_FAILURE;
}
nodeid_t target;
// 63 is broadcast. Ignore that.
if (guid < 63) {
// GUID is actually a node number.
target = LOCAL_BUS | (nodeid_t)guid;
} else {
for (unsigned no = 0; no < 63; no++) {
nodeid_t test_node = LOCAL_BUS | (nodeid_t)no;
uint32_t guid_hi;
uint32_t guid_lo;
int res = raw1394_read(fw_handle, test_node, CSR_REGISTER_BASE + CSR_CONFIG_ROM + 4*4,
4, &guid_lo);
if (res != 0) { perror("read guid_lo"); return -1; }
res = raw1394_read(fw_handle, test_node, CSR_REGISTER_BASE + CSR_CONFIG_ROM + 3*4,
4, &guid_hi);
if (res != 0) { perror("read guid_hi"); return -1; }
uint64_t test_guid = (uint64_t)ntohl(guid_hi) << 32 | ntohl(guid_lo);
if (test_guid == guid) {
target = test_node;
goto target_found;
}
}
return -1;
target_found:
;
}
quadlet_t buf[step/sizeof(quadlet_t)];
switch (mode) {
case SCREEN:
#ifndef NO_FW_SCREEN
{
if (SDL_Init(SDL_INIT_VIDEO) < 0) { perror("init sdl"); return -1; }
SDL_Surface *screen = SDL_SetVideoMode(width, height, depth, SDL_SWSURFACE);
if (!screen) { perror("sdl video mode"); return -1; }
while (true) {
quadlet_t * buf = reinterpret_cast<quadlet_t *>(screen->pixels);
for (uint64_t cur = address; cur < address+length; cur += step, buf += step/sizeof(quadlet_t)) {
size_t size = (cur + step > address+length) ? (address+length - cur) : step;
int res = raw1394_read(fw_handle, target, cur, size, buf);
if (res != 0) { perror("read data"); return EXIT_FAILURE; }
}
SDL_UpdateRect(screen, 0, 0, width, height);
SDL_Delay(500);
SDL_Event event;
if (SDL_PollEvent(&event))
if (event.type == SDL_QUIT) exit(1);
}
}
break;
#else
abort();
#endif // NO_FW_SCREEN
case PEEK:
for (uint64_t cur = address; cur < address+length; cur += step) {
size_t size = (cur + step > address+length) ? (address+length - cur) : step;
int tries = 5;
again:
int res = raw1394_read(fw_handle, target, cur, size, buf);
if (res != 0) { if (tries-- > 0) goto again; perror("read data"); return EXIT_FAILURE; }
if (write(STDOUT_FILENO, buf, size) < 0) {
perror("write");
return EXIT_FAILURE;
}
}
break;
case POKE:
for (uint64_t cur = address;;cur += step) {
ssize_t size = read(STDIN_FILENO, buf, step);
if (size == 0)
break;
if (size < 0) {
perror("read");
return EXIT_FAILURE;
}
int res = raw1394_write(fw_handle, target, cur, size, buf);
if (res != 0) { perror("write data"); return EXIT_FAILURE; }
if (size < step)
break;
}
}
return 0;
}
int
main(int argc, char **argv)
{
/* Command line parsing */
int opt;
unsigned port = 0;
unsigned step = 128;
enum { INVALID, PEEK, POKE } mode = INVALID;
const char *name = strippath(argv[0]);
if (strcmp(name, "fw_peek") == 0) {
mode = PEEK;
} else if (strcmp(name, "fw_poke") == 0) {
mode = POKE;
}
if (mode == INVALID) {
fprintf(stderr, "Could not decide, whether we are fw_peek or fw_poke.\n");
return EXIT_FAILURE;
}
while ((opt = getopt(argc, argv, "p:b:")) != -1) {
switch (opt) {
case 'p':
port = strtoul(optarg, 0, 0);
break;
case 'b':
step = strtoul(optarg, 0, 0);
break;
default:
goto print_usage;
}
}
if (((mode == PEEK) && (argc - optind) != 3) ||
((mode == POKE) && (argc - optind) != 2)) {
print_usage:
fprintf(stderr, (mode == PEEK) ? usage_peek : usage_poke, name);
return EXIT_FAILURE;
}
uint64_t guid = strtoull(argv[optind], NULL, 0);
uint64_t address = strtoull(argv[optind + 1], NULL, 0);
uint64_t length;
if (mode == PEEK) length = strtoull(argv[optind + 2], NULL, 0);
raw1394handle_t fw_handle = raw1394_new_handle_on_port(port);
if (fw_handle == NULL) {
perror("raw1394_new_handle_on_port");
return EXIT_FAILURE;
}
nodeid_t target;
// 63 is broadcast. Ignore that.
if (guid < 63) {
// GUID is actually a node number.
target = LOCAL_BUS | (nodeid_t)guid;
} else {
for (unsigned no = 0; no < 63; no++) {
nodeid_t test_node = LOCAL_BUS | (nodeid_t)no;
uint32_t guid_hi;
uint32_t guid_lo;
int res = raw1394_read(fw_handle, test_node, CSR_REGISTER_BASE + CSR_CONFIG_ROM + 4*4,
4, &guid_lo);
if (res != 0) { perror("read guid_lo"); return -1; }
res = raw1394_read(fw_handle, test_node, CSR_REGISTER_BASE + CSR_CONFIG_ROM + 3*4,
4, &guid_hi);
if (res != 0) { perror("read guid_hi"); return -1; }
uint64_t test_guid = (uint64_t)ntohl(guid_hi) << 32 | ntohl(guid_lo);
if (test_guid == guid) {
target = test_node;
goto target_found;
}
}
return -1;
target_found:
;
}
quadlet_t buf[step/sizeof(quadlet_t)];
switch (mode) {
case PEEK:
for (uint64_t cur = address; cur < address+length; cur += step) {
size_t size = (cur + step > address+length) ? (address+length - cur) : step;
int tries = 5;
again:
int res = raw1394_read(fw_handle, target, cur, size, buf);
if (res != 0) { if (tries-- > 0) goto again; perror("read data"); return EXIT_FAILURE; }
if (write(STDOUT_FILENO, buf, size) < 0) {
perror("write");
return EXIT_FAILURE;
}
}
break;
case POKE:
for (uint64_t cur = address;;cur += step) {
ssize_t size = read(STDIN_FILENO, buf, step);
if (size == 0)
break;
if (size < 0) {
perror("read");
return EXIT_FAILURE;
}
int res = raw1394_write(fw_handle, target, cur, size, buf);
if (res != 0) { perror("write data"); return EXIT_FAILURE; }
if (size < step)
break;
}
}
return 0;
}
