static void function2 (struct _file_ * file, signed index, flag_t flags)
{
static signed image = 0;
static signed origin = ~0;
static signed offset = 0;
struct nvm_header2 nvm_header;
if (read (file->file, &nvm_header, sizeof (nvm_header)) != sizeof (nvm_header))
{
error (1, errno, NVM_HDR_CANTREAD, file->name, image);
}
if (LE16TOH (nvm_header.MajorVersion) != 1)
{
error (1, 0, NVM_HDR_VERSION, file->name, image);
}
if (LE16TOH (nvm_header.MinorVersion) != 1)
{
error (1, 0, NVM_HDR_VERSION, file->name, image);
}
if (checksum32 (&nvm_header, sizeof (nvm_header), 0))
{
error (1, 0, NVM_HDR_CHECKSUM, file->name, image);
}
if (~nvm_header.PrevHeader)
{
error (1, 0, NVM_HDR_LINK, file->name, image);
}
if (~nvm_header.NextHeader)
{
error (1, 0, NVM_HDR_LINK, file->name, image);
}
nvm_header.PrevHeader = HTOLE32 (origin);
origin = offset;
if (index)
{
offset += sizeof (nvm_header);
offset += LE32TOH (nvm_header.ImageLength);
nvm_header.NextHeader = HTOLE32 (offset);
}
nvm_header.HeaderChecksum = 0;
nvm_header.HeaderChecksum = checksum32 (&nvm_header, sizeof (nvm_header), 0);
if (write (STDOUT_FILENO, &nvm_header, sizeof (nvm_header)) != sizeof (nvm_header))
{
error (1, errno, NVM_HDR_CANTSAVE, file->name, image);
}
copyimage (file, LE32TOH (nvm_header.ImageLength), image);
image++;
return;
}
static void function1 (struct _file_ * file, signed index, flag_t flags)
{
static signed image = 0;
static uint32_t offset = 0;
struct nvm_header1 nvm_header;
if (read (file->file, &nvm_header, sizeof (nvm_header)) != sizeof (nvm_header))
{
error (1, errno, NVM_HDR_CANTREAD, file->name, image);
}
if (LE32TOH (nvm_header.HEADERVERSION) != 0x60000000)
{
error (1, 0, NVM_HDR_VERSION, file->name, image);
}
if (checksum32 (&nvm_header, sizeof (nvm_header), 0))
{
error (1, 0, NVM_HDR_CHECKSUM, file->name, image);
}
if (nvm_header.NEXTHEADER)
{
error (1, 0, NVM_HDR_LINK, file->name, image);
}
if (index)
{
offset += sizeof (nvm_header);
offset += LE32TOH (nvm_header.IMAGELENGTH);
nvm_header.NEXTHEADER = HTOLE32 (offset);
}
nvm_header.HEADERCHECKSUM = 0;
nvm_header.HEADERCHECKSUM = checksum32 (&nvm_header, sizeof (nvm_header), 0);
if (write (STDOUT_FILENO, &nvm_header, sizeof (nvm_header)) != sizeof (nvm_header))
{
error (1, errno, NVM_HDR_CANTSAVE, file->name, image);
}
copyimage (file, LE32TOH (nvm_header.IMAGELENGTH), image);
return;
}
signed ReadPIB (struct int6k *int6k)
{
struct channel * channel = (struct channel *)(int6k->channel);
struct message * message = (struct message *)(int6k->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_rd_mod_request
{
struct header_eth ethernet;
struct header_int intellon;
uint8_t MODULEID;
uint8_t MACCESS;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint8_t MSECRET [16];
}
* request = (struct vs_rd_mod_request *) (message);
struct __packed vs_rd_mod_confirm
{
struct header_eth ethernet;
struct header_int intellon;
uint8_t MSTATUS;
uint8_t RESERVED1 [3];
uint8_t MODULEID;
uint8_t RESERVED;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint32_t CHKSUM;
uint8_t BUFFER [INT6K_BLOCKSIZE];
}
* confirm = (struct vs_rd_mod_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
uint32_t extent = 0;
uint32_t offset = 0;
signed length = INT6K_BLOCKSIZE;
Request (int6k, "Read Parameters from Device");
if (lseek (int6k->pib.file, 0, SEEK_SET))
{
error ((int6k->flags & INT6K_BAILOUT), errno, "Can't rewind %s", filepart (int6k->pib.name));
return (1);
}
memset (message, 0, sizeof (struct message));
do
{
EthernetHeader (&message->ethernet, channel->peer, channel->host);
IntellonHeader (&message->intellon, (VS_RD_MOD | MMTYPE_REQ));
int6k->packetsize = ETHER_MIN_LEN;
request->MODULEID = VS_MODULE_PIB;
request->MLENGTH = HTOLE16 (length);
request->MOFFSET = HTOLE32 (offset);
if (SendMME (int6k) <= 0)
{
error ((int6k->flags & INT6K_BAILOUT), ECANCELED, INT6K_CANTSEND);
return (-1);
}
if (ReadMME (int6k, (VS_RD_MOD | MMTYPE_CNF)) <= 0)
{
error ((int6k->flags & INT6K_BAILOUT), ECANCELED, INT6K_CANTREAD);
return (-1);
}
if (confirm->MSTATUS)
{
Failure (int6k, INT6K_WONTDOIT);
return (-1);
}
#if 1
if (LE16TOH (confirm->MLENGTH) != length)
{
Failure (int6k, INT6K_ERR_LENGTH);
return (-1);
}
if (LE32TOH (confirm->MOFFSET) != offset)
{
Failure (int6k, INT6K_ERR_OFFSET);
return (-1);
}
#else
if (LE16TOH (confirm->MLENGTH) != length)
{
error ((int6k->flags & INT6K_BAILOUT), 0, INT6K_ERR_LENGTH);
length = INT6K_BLOCKSIZE;
offset = 0;
continue;
}
if (LE32TOH (confirm->MOFFSET) != offset)
{
error ((int6k->flags & INT6K_BAILOUT), 0, INT6K_ERR_OFFSET);
length = INT6K_BLOCKSIZE;
offset = 0;
continue;
}
#endif
length = LE16TOH (confirm->MLENGTH);
offset = LE32TOH (confirm->MOFFSET);
if (checksum_32 (confirm->BUFFER, length, confirm->CHKSUM))
{
error ((int6k->flags & INT6K_BAILOUT), ECANCELED, "Bad Packet Checksum");
return (-1);
}
if (offset == extent)
{
struct header_pib * header_pib = (struct header_pib *) (confirm->BUFFER);
extent = header_pib->PIBLENGTH;
}
if ((offset + length) > extent)
{
length = extent - offset;
}
if (lseek (int6k->pib.file, offset, SEEK_SET) != offset)
{
error ((int6k->flags & INT6K_BAILOUT), errno, "can't seek %s", filepart (int6k->pib.name));
return (-1);
}
if (write (int6k->pib.file, confirm->BUFFER, length) < length)
{
error ((int6k->flags & INT6K_BAILOUT), errno, "can't save %s", filepart (int6k->pib.name));
return (-1);
}
offset += length;
}
while (offset < extent);
Confirm (int6k, "Read %s", int6k->pib.name);
return (0);
}
signed ModuleWrite (struct plc * plc, struct _file_ * file, unsigned index, struct vs_module_spec * vs_module_spec)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_module_operation_write_request
{
struct ethernet_std ethernet;
struct qualcomm_std qualcomm;
uint32_t RESERVED;
uint8_t NUM_OP_DATA;
struct __packed
{
uint16_t MOD_OP;
uint16_t MOD_OP_DATA_LEN;
uint32_t MOD_OP_RSVD;
uint32_t MOD_OP_SESSION_ID;
uint8_t MODULE_IDX;
uint16_t MODULE_ID;
uint16_t MODULE_SUB_ID;
uint16_t MODULE_LENGTH;
uint32_t MODULE_OFFSET;
}
MODULE_SPEC;
uint8_t MODULE_DATA [PLC_MODULE_SIZE];
}
* request = (struct vs_module_operation_write_request *)(message);
struct __packed vs_module_operation_write_confirm
{
struct ethernet_std ethernet;
struct qualcomm_std qualcomm;
uint16_t MSTATUS;
uint16_t ERR_REC_CODE;
uint32_t RESERVED;
uint8_t NUM_OP_DATA;
struct __packed
{
uint16_t MOD_OP;
uint16_t MOD_OP_DATA_LEN;
uint32_t MOD_OP_RSVD;
uint32_t MOD_OP_SESSION_ID;
uint8_t MODULE_IDX;
uint16_t MODULE_ID;
uint16_t MODULE_SUB_ID;
uint16_t MODULE_LENGTH;
uint32_t MODULE_OFFSET;
}
MODULE_SPEC;
}
* confirm = (struct vs_module_operation_write_confirm *)(message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
unsigned timeout = channel->timeout;
uint16_t length = PLC_MODULE_SIZE;
uint32_t extent = vs_module_spec->MODULE_LENGTH;
uint32_t offset = 0;
Request (plc, "Flash %s", file->name);
while (extent)
{
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, HOMEPLUG_MTYPE);
QualcommHeader (&request->qualcomm, 0, (VS_MODULE_OPERATION | MMTYPE_REQ));
plc->packetsize = sizeof (struct vs_module_operation_write_request);
if (length > extent)
{
length = extent;
}
if (read (file->file, request->MODULE_DATA, length) != length)
{
error (1, errno, FILE_CANTREAD, file->name);
}
request->NUM_OP_DATA = 1;
request->MODULE_SPEC.MOD_OP = HTOLE16 (PLC_MOD_OP_WRITE_MODULE);
request->MODULE_SPEC.MOD_OP_DATA_LEN = HTOLE16 (sizeof (request->MODULE_SPEC) + sizeof (request->MODULE_DATA));
request->MODULE_SPEC.MOD_OP_SESSION_ID = HTOLE32 (plc->cookie);
request->MODULE_SPEC.MODULE_IDX = index;
request->MODULE_SPEC.MODULE_ID = HTOLE16 (vs_module_spec->MODULE_ID);
request->MODULE_SPEC.MODULE_SUB_ID = HTOLE16 (vs_module_spec->MODULE_SUB_ID);
request->MODULE_SPEC.MODULE_LENGTH = HTOLE16 (length);
request->MODULE_SPEC.MODULE_OFFSET = HTOLE32 (offset);
#if 0
fprintf (stderr, "RESERVED 0x%08X\n", LE32TOH (request->RESERVED));
fprintf (stderr, "NUM_OP_DATA %d\n", request->NUM_OP_DATA);
fprintf (stderr, "MOD_OP 0x%02X\n", LE16TOH (request->MODULE_SPEC.MOD_OP));
fprintf (stderr, "MOD_OP_DATA_LEN %d\n", LE16TOH (request->MODULE_SPEC.MOD_OP_DATA_LEN));
fprintf (stderr, "RESERVED 0x%08X\n", LE32TOH (request->MODULE_SPEC.MOD_OP_RSVD));
fprintf (stderr, "MODULE_ID 0x%04X\n", LE16TOH (request->MODULE_SPEC.MODULE_ID));
fprintf (stderr, "MODULE_SUB_ID 0x%04X\n", LE16TOH (request->MODULE_SPEC.MODULE_SUB_ID));
fprintf (stderr, "MODULE_LENGTH %d\n", LE16TOH (request->MODULE_SPEC.MODULE_LENGTH));
fprintf (stderr, "MODULE_OFFSET 0x%08X\n", LE32TOH (request->MODULE_SPEC.MODULE_OFFSET));
#endif
if (SendMME (plc) <= 0)
{
error ((plc->flags & PLC_BAILOUT), errno, CHANNEL_CANTSEND);
return (-1);
}
channel->timeout = PLC_MODULE_WRITE_TIMEOUT;
if (ReadMME (plc, 0, (VS_MODULE_OPERATION | MMTYPE_CNF)) <= 0)
{
error ((plc->flags & PLC_BAILOUT), errno, CHANNEL_CANTREAD);
channel->timeout = timeout;
return (-1);
}
channel->timeout = timeout;
#if 0
fprintf (stderr, "MSTATUS 0x%04X\n", LE16TOH (confirm->MSTATUS));
fprintf (stderr, "ERROR_REC_CODE %d\n", LE16TOH (confirm->ERR_REC_CODE));
fprintf (stderr, "RESERVED 0x%08X\n", LE32TOH (confirm->RESERVED));
fprintf (stderr, "NUM_OP_DATA %d\n", confirm->NUM_OP_DATA);
fprintf (stderr, "MOD_OP 0x%02X\n", LE16TOH (request->MODULE_SPEC.MOD_OP));
fprintf (stderr, "MOD_OP_DATA_LEN %d\n", LE16TOH (confirm->MODULE_SPEC.MOD_OP_DATA_LEN));
fprintf (stderr, "RESERVED 0x%08X\n", LE32TOH (confirm->MODULE_SPEC.MOD_OP_RSVD));
fprintf (stderr, "MODULE_ID 0x%04X\n", LE16TOH (confirm->MODULE_SPEC.MODULE_ID));
fprintf (stderr, "MODULE_SUB_ID 0x%04X\n", LE16TOH (confirm->MODULE_SPEC.MODULE_SUB_ID));
fprintf (stderr, "MODULE_LENGTH %d\n", LE16TOH (confirm->MODULE_SPEC.MODULE_LENGTH));
fprintf (stderr, "MODULE_OFFSET 0x%08X\n", LE32TOH (request->MODULE_SPEC.MODULE_OFFSET));
#endif
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
return (-1);
}
if (LE16TOH (confirm->MODULE_SPEC.MODULE_LENGTH) != length)
{
error ((plc->flags & PLC_BAILOUT), 0, PLC_ERR_LENGTH);
return (-1);
}
if (LE32TOH (confirm->MODULE_SPEC.MODULE_OFFSET) != offset)
{
error ((plc->flags & PLC_BAILOUT), 0, PLC_ERR_OFFSET);
return (-1);
}
extent -= length;
offset += length;
}
return (0);
}
signed WriteExecuteFirmware1 (struct plc * plc, unsigned module, const struct nvm_header1 * nvm_header)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_write_execute_request
{
struct ethernet_std ethernet;
struct qualcomm_std qualcomm;
uint32_t CLIENT_SESSION_ID;
uint32_t SERVER_SESSION_ID;
uint32_t FLAGS;
uint8_t ALLOWED_MEM_TYPES [8];
uint32_t TOTAL_LENGTH;
uint32_t CURR_PART_LENGTH;
uint32_t CURR_PART_OFFSET;
uint32_t START_ADDR;
uint32_t CHECKSUM;
uint8_t RESERVED2 [8];
uint8_t IMAGE [PLC_MODULE_SIZE];
}
* request = (struct vs_write_execute_request *) (message);
struct __packed vs_write_execute_confirm
{
struct ethernet_std ethernet;
struct qualcomm_std qualcomm;
uint32_t MSTATUS;
uint32_t CLIENT_SESSION_ID;
uint32_t SERVER_SESSION_ID;
uint32_t FLAGS;
uint8_t ALLOWED_MEM_TYPES [8];
uint32_t TOTAL_LENGTH;
uint32_t CURR_PART_LENGTH;
uint32_t CURR_PART_OFFSET;
uint32_t START_ADDR;
uint32_t CHECKSUM;
uint8_t RESERVED2 [8];
uint32_t CURR_PART_ABSOLUTE_ADDR;
uint32_t ABSOLUTE_START_ADDR;
}
* confirm = (struct vs_write_execute_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
unsigned action = PLC_MODULE_ABSOLUTE;
uint32_t length = PLC_MODULE_SIZE;
uint32_t offset = LE32TOH (nvm_header->IMAGEADDRESS);
uint32_t extent = LE32TOH (nvm_header->IMAGELENGTH);
Request (plc, "Write %s (%d) (%08X:%d)", plc->NVM.name, module, offset, extent);
while (extent)
{
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_WRITE_AND_EXECUTE_APPLET | MMTYPE_REQ));
if (length > extent)
{
Request (plc, "Start %s (%d) (%08X)", plc->NVM.name, module, LE32TOH (nvm_header->ENTRYPOINT));
length = extent;
action |= PLC_MODULE_EXECUTE;
}
if (read (plc->NVM.file, request->IMAGE, length) != (signed)(length))
{
error (1, errno, FILE_CANTREAD, plc->NVM.name);
}
request->CLIENT_SESSION_ID = HTOLE32 (plc->cookie);
request->SERVER_SESSION_ID = HTOLE32 (0);
request->FLAGS = HTOLE32 (action);
request->ALLOWED_MEM_TYPES [0] = 1;
request->TOTAL_LENGTH = nvm_header->IMAGELENGTH;
request->CURR_PART_LENGTH = HTOLE32 (length);
request->CURR_PART_OFFSET = HTOLE32 (offset);
request->START_ADDR = nvm_header->ENTRYPOINT;
request->CHECKSUM = nvm_header->IMAGECHECKSUM;
plc->packetsize = sizeof (* request);
if (SendMME (plc) <= 0)
{
error ((plc->flags & PLC_BAILOUT), errno, CHANNEL_CANTSEND);
return (-1);
}
if (ReadMME (plc, 0, (VS_WRITE_AND_EXECUTE_APPLET | MMTYPE_CNF)) <= 0)
{
error ((plc->flags & PLC_BAILOUT), errno, CHANNEL_CANTREAD);
return (-1);
}
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
return (-1);
}
if (LE32TOH (confirm->CURR_PART_LENGTH) != length)
{
error ((plc->flags & PLC_BAILOUT), 0, PLC_ERR_LENGTH);
return (-1);
}
if (LE32TOH (confirm->CURR_PART_OFFSET) != offset)
{
error ((plc->flags & PLC_BAILOUT), 0, PLC_ERR_OFFSET);
return (-1);
}
offset += length;
extent -= length;
}
return (0);
}
signed SetProperty (struct plc * plc, struct plcproperty * plcproperty)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_set_property_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint32_t COOKIE;
uint8_t OPTION;
uint8_t RESERVED [3];
uint32_t PROP_VERSION;
uint32_t PROP_NUMBER;
uint32_t DATA_LENGTH;
uint8_t DATA_BUFFER [256];
}
* request = (struct vs_set_property_request *) (message);
struct __packed vs_set_property_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint32_t MSTATUS;
uint32_t COOKIE;
}
* confirm = (struct vs_set_property_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
Request (plc, "Set Property");
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_SET_PROPERTY | MMTYPE_REQ));
request->COOKIE = HTOLE32 (plc->cookie);
request->OPTION = plcproperty->PROP_OPTION;
request->PROP_VERSION = HTOLE32 (plcproperty->PROP_VERSION);
request->PROP_NUMBER = HTOLE32 (plcproperty->PROP_NUMBER);
request->DATA_LENGTH = HTOLE32 (plcproperty->DATA_LENGTH);
memcpy (&request->DATA_BUFFER, &plcproperty->DATA_BUFFER, plcproperty->DATA_LENGTH);
plc->packetsize = sizeof (* request);
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
while (ReadMME (plc, 0, (VS_SET_PROPERTY | MMTYPE_CNF)) > 0)
{
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
continue;
}
}
return (0);
}
void set32bitmap (uint32_t * map, unsigned bit)
{
map [bit / _bits (* map)] |= HTOLE32 (1 << (bit % _bits (* map)));
return;
}
signed ReadParameterBlock (struct plc * plc, void * memory, size_t extent)
{
uint8_t * buffer = (uint8_t *)(memory);
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_rd_mod_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MODULEID;
uint8_t MACCESS;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint8_t MSECRET [16];
}
* request = (struct vs_rd_mod_request *) (message);
struct __packed vs_rd_mod_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MSTATUS;
uint8_t RESERVED1 [3];
uint8_t MODULEID;
uint8_t RESERVED;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint32_t CHKSUM;
uint8_t BUFFER [PLC_RECORD_SIZE];
}
* confirm = (struct vs_rd_mod_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
uint16_t length = 0;
uint32_t offset = 0;
signed actual = PLC_RECORD_SIZE;
do
{
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_RD_MOD | MMTYPE_REQ));
request->MODULEID = VS_MODULE_PIB;
request->MLENGTH = HTOLE16 (actual);
request->MOFFSET = HTOLE32 (offset);
plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
if (ReadMME (plc, 0, (VS_RD_MOD | MMTYPE_CNF)) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTREAD);
return (-1);
}
if (confirm->MSTATUS)
{
error (PLC_EXIT (plc), ECANCELED, PLC_WONTDOIT);
return (-1);
}
if (LE16TOH (confirm->MLENGTH) != actual)
{
Failure (plc, PLC_ERR_LENGTH);
return (-1);
}
if (LE32TOH (confirm->MOFFSET) != offset)
{
Failure (plc, PLC_ERR_OFFSET);
return (-1);
}
actual = LE16TOH (confirm->MLENGTH);
offset = LE32TOH (confirm->MOFFSET);
if (checksum32 (confirm->BUFFER, actual, confirm->CHKSUM))
{
error (PLC_EXIT (plc), ECANCELED, "Bad Packet Checksum");
return (-1);
}
if (offset == length)
{
struct pib_header * pib_header = (struct pib_header *) (confirm->BUFFER);
length = LE16TOH (pib_header->PIBLENGTH);
}
if ((offset + actual) > length)
{
actual = length - offset;
}
memcpy (buffer + offset, confirm->BUFFER, actual);
offset += actual;
extent -= actual;
}
while (offset < length);
return (offset);
}
uint8_t LINEFREQ;
uint32_t SDRAMSIZE;
uint8_t AUTHMODE;
}
* attributes = (struct attributes *)(confirm->MBUFFER);
#ifndef __GNUC__
#pragma pack (pop)
#endif
Request (plc, "Fetch Device Attributes");
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_OP_ATTRIBUTES | MMTYPE_REQ));
plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
request->COOKIE = HTOLE32 (plc->cookie);
request->RTYPE = 0;
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
while (ReadMME (plc, 0, (VS_OP_ATTRIBUTES | MMTYPE_CNF)) > 0)
{
static char const * frequency [] =
{
"Unknown frequency",
"50Hz",
"60Hz",
};
static char const * zero_cross [] =
signed WriteExecuteParameters1 (struct plc * plc, unsigned module, const struct nvm_header1 * nvm_header)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct nvm_header1 nvm_manifest;
struct __packed vs_write_execute_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint32_t CLIENT_SESSION_ID;
uint32_t SERVER_SESSION_ID;
uint32_t FLAGS;
uint8_t ALLOWED_MEM_TYPES [8];
uint32_t TOTAL_LENGTH;
uint32_t CURR_PART_LENGTH;
uint32_t CURR_PART_OFFSET;
uint32_t START_ADDR;
uint32_t CHECKSUM;
uint8_t RESERVED2 [8];
uint8_t IMAGE [PLC_MODULE_SIZE];
}
* request = (struct vs_write_execute_request *) (message);
struct __packed vs_write_execute_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint32_t MSTATUS;
uint32_t CLIENT_SESSION_ID;
uint32_t SERVER_SESSION_ID;
uint32_t FLAGS;
uint8_t ALLOWED_MEM_TYPES [8];
uint32_t TOTAL_LENGTH;
uint32_t CURR_PART_LENGTH;
uint32_t CURR_PART_OFFSET;
uint32_t START_ADDR;
uint32_t CHECKSUM;
uint8_t RESERVED2 [8];
uint32_t CURR_PART_ABSOLUTE_ADDR;
uint32_t ABSOLUTE_START_ADDR;
}
* confirm = (struct vs_write_execute_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
uint32_t length = PLC_MODULE_SIZE;
uint32_t offset = LE32TOH (nvm_header->IMAGEADDRESS);
uint32_t extent = LE32TOH (nvm_header->IMAGELENGTH);
#if 0
Request (plc, "Write %s (%d) (%08X:%d)", plc->PIB.name, module, offset, extent);
#else
/*
* adjust the file extent to include the the manifest image header, the manifest image, the PIB
* image header and the PIB image; this should in principle, equal the PIB filesize for Panther
* and Lynx .pib files;
*/
if (lseek (plc->PIB.file, 0, SEEK_SET))
{
error (1, errno, FILE_CANTHOME, plc->PIB.name);
}
if (read (plc->PIB.file, &nvm_manifest, sizeof (nvm_manifest)) != sizeof (nvm_manifest))
{
error (1, errno, FILE_CANTREAD, plc->PIB.name);
}
extent += sizeof (* nvm_header);
extent += LE32TOH (nvm_manifest.IMAGELENGTH);
extent += sizeof (* nvm_header);
Request (plc, "Write %s (%d) (%08X:%d)", plc->PIB.name, module, offset, extent);
#endif
while (extent)
{
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_WRITE_AND_EXECUTE_APPLET | MMTYPE_REQ));
if (length > extent)
{
length = extent;
}
if (read (plc->PIB.file, request->IMAGE, length) != (signed)(length))
{
error (1, errno, FILE_CANTREAD, plc->PIB.name);
}
request->CLIENT_SESSION_ID = HTOLE32 (plc->cookie);
request->SERVER_SESSION_ID = HTOLE32 (0);
request->FLAGS = HTOLE32 (PLC_MODULE_ABSOLUTE);
request->ALLOWED_MEM_TYPES [0] = 1;
request->TOTAL_LENGTH = nvm_header->IMAGELENGTH;
request->CURR_PART_LENGTH = HTOLE32 (length);
request->CURR_PART_OFFSET = HTOLE32 (offset);
request->START_ADDR = nvm_header->ENTRYPOINT;
request->CHECKSUM = nvm_header->IMAGECHECKSUM;
plc->packetsize = sizeof (* request);
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
if (ReadMME (plc, 0, (VS_WRITE_AND_EXECUTE_APPLET | MMTYPE_CNF)) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTREAD);
return (-1);
}
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
return (-1);
}
if (LE32TOH (confirm->CURR_PART_LENGTH) != length)
{
error (PLC_EXIT (plc), 0, PLC_ERR_LENGTH);
return (-1);
}
if (LE32TOH (confirm->CURR_PART_OFFSET) != offset)
{
error (PLC_EXIT (plc), 0, PLC_ERR_OFFSET);
return (-1);
}
offset += length;
extent -= length;
}
return (0);
}
signed WritePIB (struct plc * plc)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_wr_mod_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MODULEID;
uint8_t RESERVED;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint32_t MCHKSUM;
uint8_t MBUFFER [PLC_RECORD_SIZE];
}
* request = (struct vs_wr_mod_request *) (message);
struct __packed vs_wr_mod_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MSTATUS;
uint8_t MODULEID;
uint8_t RESERVED;
uint16_t MLENGTH;
uint32_t MOFFSET;
}
* confirm = (struct vs_wr_mod_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
uint16_t length = PLC_RECORD_SIZE;
uint32_t extent = lseek (plc->PIB.file, 0, SEEK_END);
uint32_t offset = lseek (plc->PIB.file, 0, SEEK_SET);
Request (plc, "Write %s to scratch", plc->PIB.name);
while (extent)
{
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_WR_MOD | MMTYPE_REQ));
if (length > extent)
{
length = extent;
}
if (read (plc->PIB.file, request->MBUFFER, length) != length)
{
error (1, errno, FILE_CANTREAD, plc->PIB.name);
}
request->MODULEID = VS_MODULE_PIB;
request->RESERVED = 0;
request->MLENGTH = HTOLE16 (length);
request->MOFFSET = HTOLE32 (offset);
request->MCHKSUM = checksum32 (request->MBUFFER, length, 0);
plc->packetsize = sizeof (* request);
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
if (ReadMME (plc, 0, (VS_WR_MOD | MMTYPE_CNF)) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTREAD);
return (-1);
}
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
return (-1);
}
if (LE16TOH (confirm->MLENGTH) != length)
{
error (PLC_EXIT (plc), 0, PLC_ERR_LENGTH);
length = PLC_RECORD_SIZE;
offset = 0;
continue;
}
if (LE32TOH (confirm->MOFFSET) != offset)
{
error (PLC_EXIT (plc), 0, PLC_ERR_OFFSET);
length = PLC_RECORD_SIZE;
offset = 0;
continue;
}
extent -= length;
offset += length;
}
return (0);
}
size_t memencode (void * memory, size_t extent, char const * format, char const * string)
{
if (!strcmp (format, "byte"))
{
uint8_t * number = (uint8_t *)(memory);
if (extent < sizeof (* number))
{
error (1, ECANCELED, "Overflow at %s %s", format, string);
}
* number = (uint8_t)(basespec (string, 0, sizeof (* number)));
return (sizeof (* number));
}
if (!strcmp (format, "word"))
{
uint16_t * number = (uint16_t *)(memory);
if (extent < sizeof (* number))
{
error (1, ECANCELED, "Overflow at %s %s", format, string);
}
* number = (uint16_t)(basespec (string, 0, sizeof (* number)));
* number = HTOLE16 (* number);
return (sizeof (* number));
}
if (!strcmp (format, "long"))
{
uint32_t * number = (uint32_t *)(memory);
if (extent < sizeof (* number))
{
error (1, ECANCELED, "Overflow at %s %s", format, string);
}
* number = (uint32_t)(basespec (string, 0, sizeof (* number)));
* number = HTOLE32 (* number);
return (sizeof (* number));
}
if (!strcmp (format, "huge"))
{
uint64_t * number = (uint64_t *)(memory);
if (extent < sizeof (* number))
{
error (1, ECANCELED, "Overflow at %s %s", format, string);
}
* number = (uint64_t)(basespec (string, 0, sizeof (* number)));
* number = HTOLE64 (* number);
return (sizeof (* number));
}
if (!strcmp (format, "text"))
{
extent = (unsigned)(strlen (string));
memcpy (memory, string, extent);
return (extent);
}
if (!strcmp (format, "data"))
{
extent = (unsigned)(dataspec (string, memory, extent));
return (extent);
}
if (!strcmp (format, "fill"))
{
extent = (unsigned)(uintspec (string, 0, extent));
memset (memory, ~0, extent);
return (extent);
}
if (!strcmp (format, "zero"))
{
extent = (unsigned)(uintspec (string, 0, extent));
memset (memory, 0, extent);
return (extent);
}
if (!strcmp (format, "skip"))
{
extent = (unsigned)(uintspec (string, 0, extent));
return (extent);
}
#if 1
/*
* tr-069 specific fields that don't really belong in the PIB;
*/
if (!strcmp (format, "adminusername") || !strcmp (format, "adminpassword") || !strcmp (format, "accessusername"))
{
return (memstring (memory, extent, format, string, PIB_NAME_LEN + 1));
}
if (!strcmp (format, "accesspassword"))
{
return (memstring (memory, extent, format, string, PIB_HFID_LEN + 1));
}
if (!strcmp (format, "username") || !strcmp (format, "password") || !strcmp (format, "url"))
{
return (memstring (memory, extent, format, string, PIB_TEXT_LEN + 1));
}
#endif
#if 1
/*
* HPAV specific fields that belong in the PIB;
*/
if (!strcmp (format, "hfid"))
{
return (memstring (memory, extent, format, string, PIB_HFID_LEN));
}
if (!strcmp (format, "mac"))
{
return (bytespec (string, memory, ETHER_ADDR_LEN));
}
if (!strcmp (format, "key"))
{
return (bytespec (string, memory, PIB_KEY_LEN));
}
#endif
error (1, ENOTSUP, "%s", format);
return (0);
}
signed GetProperty (struct plc * plc, struct plcproperty * plcproperty)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_get_property_request
{
struct ethernet_std ethernet;
struct qualcomm_std qualcomm;
uint32_t COOKIE;
uint8_t DATA_FORMAT;
uint8_t PROP_FORMAT;
uint8_t RESERVED [2];
uint32_t PROP_VERSION;
uint32_t PROP_LENGTH;
uint8_t PROP_NUMBER;
}
* request = (struct vs_get_property_request *) (message);
struct __packed vs_get_property_confirm
{
struct ethernet_std ethernet;
struct qualcomm_std qualcomm;
uint32_t MSTATUS;
uint32_t COOKIE;
uint8_t DATA_FORMAT;
uint8_t RESERVED [3];
uint32_t DATA_LENGTH;
uint32_t DATA_BUFFER [1];
}
* confirm = (struct vs_get_property_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
Request (plc, "Get Property");
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, HOMEPLUG_MTYPE);
QualcommHeader (&request->qualcomm, 0, (VS_GET_PROPERTY | MMTYPE_REQ));
request->COOKIE = HTOLE32 (plc->cookie);
request->DATA_FORMAT = plcproperty->DATA_FORMAT;
request->PROP_FORMAT = plcproperty->PROP_FORMAT;
request->PROP_VERSION = HTOLE32 (plcproperty->PROP_VERSION);
request->PROP_LENGTH = HTOLE32 (plcproperty->PROP_LENGTH);
request->PROP_NUMBER = HTOLE32 (plcproperty->PROP_NUMBER);
plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
if (SendMME (plc) <= 0)
{
error ((plc->flags & PLC_BAILOUT), errno, CHANNEL_CANTSEND);
return (-1);
}
while (ReadMME (plc, 0, (VS_GET_PROPERTY | MMTYPE_CNF)) > 0)
{
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
continue;
}
if (plcproperty->DATA_FORMAT == PLC_FORMAT_BIN)
{
binout (confirm->DATA_BUFFER, confirm->DATA_LENGTH, ' ', '\n', stdout);
continue;
}
if (plcproperty->DATA_FORMAT == PLC_FORMAT_HEX)
{
hexout (confirm->DATA_BUFFER, confirm->DATA_LENGTH, ' ', '\n', stdout);
continue;
}
if (plcproperty->DATA_FORMAT == PLC_FORMAT_DEC)
{
decout (confirm->DATA_BUFFER, confirm->DATA_LENGTH, ' ', '\n', stdout);
continue;
}
if (plcproperty->DATA_FORMAT == PLC_FORMAT_ASC)
{
chrout (confirm->DATA_BUFFER, confirm->DATA_LENGTH, '.', '\n', stdout);
continue;
}
}
return (0);
}
static void ReadKey1 (struct channel * channel, unsigned c, int key)
{
struct message message;
static signed count = 0;
signed packetsize;
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_rd_mod_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MODULEID;
uint8_t RESERVED;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint8_t DAK [16];
}
* request = (struct vs_rd_mod_request *)(&message);
struct __packed vs_rd_mod_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MSTATUS;
uint8_t RESERVED1 [3];
uint8_t MODULEID;
uint8_t RESERVED2;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint32_t MCHKSUM;
struct simple_pib pib;
}
* confirm = (struct vs_rd_mod_confirm *)(&message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
memset (&message, 0, sizeof (message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_RD_MOD | MMTYPE_REQ));
request->MODULEID = VS_MODULE_PIB;
request->MLENGTH = HTOLE16 (PLC_RECORD_SIZE);
request->MOFFSET = HTOLE32 (0);
if (sendpacket (channel, &message, (ETHER_MIN_LEN - ETHER_CRC_LEN)) < 0)
{
error (1, errno, CHANNEL_CANTSEND);
}
while ((packetsize = readpacket (channel, &message, sizeof (message))) > 0)
{
if (UnwantedMessage (&message, packetsize, 0, (VS_RD_MOD | MMTYPE_CNF)))
{
continue;
}
if (confirm->MSTATUS)
{
error (0, 0, "%s (%0X): ", MMECode (confirm->qualcomm.MMTYPE, confirm->MSTATUS), confirm->MSTATUS);
continue;
}
if (count++ > 0)
{
putc (c, stdout);
}
if (key == INT6KID_MAC)
{
hexout (confirm->pib.MAC, sizeof (confirm->pib.MAC), HEX_EXTENDER, 0, stdout);
continue;
}
if (key == INT6KID_DAK)
{
hexout (confirm->pib.DAK, sizeof (confirm->pib.DAK), HEX_EXTENDER, 0, stdout);
continue;
}
if (key == INT6KID_NMK)
{
hexout (confirm->pib.NMK, sizeof (confirm->pib.NMK), HEX_EXTENDER, 0, stdout);
continue;
}
if (key == INT6KID_MFG)
{
confirm->pib.MFG [PIB_HFID_LEN - 1] = (char)(0);
printf ("%s", confirm->pib.MFG);
continue;
}
if (key == INT6KID_USR)
{
confirm->pib.USR [PIB_HFID_LEN - 1] = (char)(0);
printf ("%s", confirm->pib.USR);
continue;
}
if (key == INT6KID_NET)
{
confirm->pib.NET [PIB_HFID_LEN - 1] = (char)(0);
printf ("%s", confirm->pib.NET);
continue;
}
}
if (packetsize < 0)
{
error (1, errno, CHANNEL_CANTREAD);
}
return;
}
signed ModuleDump (struct plc * plc, uint16_t source, uint16_t module, uint16_t submodule)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_module_operation_read_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint32_t RESERVED;
uint8_t NUM_OP_DATA;
struct __packed
{
uint16_t MOD_OP;
uint16_t MOD_OP_DATA_LEN;
uint32_t MOD_OP_RSVD;
uint16_t MODULE_ID;
uint16_t MODULE_SUB_ID;
uint16_t MODULE_LENGTH;
uint32_t MODULE_OFFSET;
}
MODULE_SPEC;
}
* request = (struct vs_module_operation_read_request *)(message);
struct __packed vs_module_operation_read_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint16_t MSTATUS;
uint16_t ERR_REC_CODE;
uint32_t RESERVED;
uint8_t NUM_OP_DATA;
struct __packed
{
uint16_t MOD_OP;
uint16_t MOD_OP_DATA_LEN;
uint32_t MOD_OP_RSVD;
uint16_t MODULE_ID;
uint16_t MODULE_SUB_ID;
uint16_t MODULE_LENGTH;
uint32_t MODULE_OFFSET;
}
MODULE_SPEC;
uint8_t MODULE_DATA [PLC_MODULE_SIZE];
}
* confirm = (struct vs_module_operation_read_confirm *)(message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
unsigned offset = 0;
unsigned length = PLC_MODULE_SIZE;
unsigned timer = channel->timeout;
Request (plc, "Read Module from Flash");
while (length == PLC_MODULE_SIZE)
{
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_MODULE_OPERATION | MMTYPE_REQ));
plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
request->NUM_OP_DATA = 1;
request->MODULE_SPEC.MOD_OP = HTOLE16 (source);
request->MODULE_SPEC.MOD_OP_DATA_LEN = HTOLE16 (sizeof (request->MODULE_SPEC));
request->MODULE_SPEC.MOD_OP_RSVD = HTOLE32 (0);
request->MODULE_SPEC.MODULE_ID = HTOLE16 (module);
request->MODULE_SPEC.MODULE_SUB_ID = HTOLE16 (submodule);
request->MODULE_SPEC.MODULE_LENGTH = HTOLE16 (length);
request->MODULE_SPEC.MODULE_OFFSET = HTOLE32 (offset);
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
channel->timeout = PLC_MODULE_READ_TIMEOUT;
if (ReadMME (plc, 0, (VS_MODULE_OPERATION | MMTYPE_CNF)) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTREAD);
channel->timeout = timer;
return (-1);
}
channel->timeout = timer;
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
return (-1);
}
length = LE16TOH (confirm->MODULE_SPEC.MODULE_LENGTH);
offset = LE32TOH (confirm->MODULE_SPEC.MODULE_OFFSET);
hexview (confirm->MODULE_DATA, LE32TOH (confirm->MODULE_SPEC.MODULE_OFFSET), LE16TOH (confirm->MODULE_SPEC.MODULE_LENGTH), stdout);
offset += length;
}
return (0);
}
signed ModuleSession (struct plc * plc, unsigned modules, struct vs_module_spec * vs_module_spec)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_module_operation_start_request
{
struct ethernet_std ethernet;
struct qualcomm_std qualcomm;
uint32_t RESERVED1;
uint8_t NUM_OP_DATA;
struct __packed
{
uint16_t MOD_OP;
uint16_t MOD_OP_DATA_LEN;
uint32_t MOD_OP_RSVD;
uint32_t MOD_OP_SESSION_ID;
uint8_t NUM_MODULES;
}
MODULE_SPEC;
struct vs_module_spec MOD_OP_SPEC [10];
}
* request = (struct vs_module_operation_start_request *)(message);
struct __packed vs_module_operation_start_confirm
{
struct ethernet_std ethernet;
struct qualcomm_std qualcomm;
uint16_t MSTATUS;
uint16_t ERR_REC_CODE;
uint32_t RESERVED;
uint8_t NUM_OP_DATA;
struct __packed
{
uint16_t MOD_OP;
uint16_t MOD_OP_DATA_LEN;
uint32_t MOD_OP_RSVD;
uint32_t MOD_OP_SESSION_ID;
uint8_t NUM_MODULES;
}
MODULE_SPEC;
struct __packed
{
uint16_t MOD_STATUS;
uint16_t ERR_REC_CODE;
}
MOD_OP_DATA [1];
}
* confirm = (struct vs_module_operation_start_confirm *)(message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
unsigned timer = channel->timeout;
struct vs_module_spec * request_spec = (struct vs_module_spec *)(&request->MOD_OP_SPEC);
Request (plc, "Start Module Write Session");
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_MODULE_OPERATION | MMTYPE_REQ));
plc->packetsize = sizeof (* request);
request->NUM_OP_DATA = 1;
request->MODULE_SPEC.MOD_OP = HTOLE16 (PLC_MOD_OP_START_SESSION);
request->MODULE_SPEC.MOD_OP_DATA_LEN = HTOLE16 ((uint16_t)(sizeof (request->MODULE_SPEC)) + modules * sizeof (struct vs_module_spec));
request->MODULE_SPEC.MOD_OP_SESSION_ID = HTOLE32 (plc->cookie);
request->MODULE_SPEC.NUM_MODULES = modules;
while (modules--)
{
request_spec->MODULE_ID = HTOLE16 (vs_module_spec->MODULE_ID);
request_spec->MODULE_SUB_ID = HTOLE16 (vs_module_spec->MODULE_SUB_ID);
request_spec->MODULE_LENGTH = HTOLE32 (vs_module_spec->MODULE_LENGTH);
request_spec->MODULE_CHKSUM = vs_module_spec->MODULE_CHKSUM;
vs_module_spec++;
request_spec++;
}
if (SendMME (plc) <= 0)
{
error ((plc->flags & PLC_BAILOUT), errno, CHANNEL_CANTSEND);
return (-1);
}
channel->timeout = PLC_MODULE_REQUEST_TIMEOUT;
if (ReadMME (plc, 0, (VS_MODULE_OPERATION | MMTYPE_CNF)) <= 0)
{
error ((plc->flags & PLC_BAILOUT), errno, CHANNEL_CANTREAD);
channel->timeout = timer;
return (-1);
}
channel->timeout = timer;
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
return (-1);
}
return (0);
}
u_int32_t
gzip_global(u_int8_t *data, u_int32_t size,
int decomp, u_int8_t **out, int size_hint)
{
/* decomp indicates whether we compress (0) or decompress (1) */
z_stream zbuf;
u_int8_t *output;
u_int32_t count, result;
int error, i, j;
struct deflate_buf buf[ZBUF];
u_int32_t crc;
u_int32_t isize = 0, icrc = 0;
DPRINTF(("gzip_global: decomp %d, size %d\n", decomp, size));
memset(&zbuf, 0, sizeof(z_stream));
zbuf.zalloc = ocf_zalloc;
zbuf.zfree = ocf_zfree;
zbuf.opaque = Z_NULL;
if (!decomp) {
/* compress */
DPRINTF(("gzip_global: compress malloc %d + %d + %d = %d\n",
size, sizeof(gzip_header), GZIP_TAIL_SIZE,
size + sizeof(gzip_header) + GZIP_TAIL_SIZE));
buf[0].size = size;
crc = crc32(0, data, size);
DPRINTF(("gzip_compress: size %d, crc 0x%x\n", size, crc));
zbuf.avail_in = size; /* Total length of data to be processed */
zbuf.next_in = data; /* data that is going to be processed */
} else {
/* decompress */
/* check the gzip header */
if (size <= sizeof(gzip_header) + GZIP_TAIL_SIZE) {
/* Not enough data for the header & tail */
DPRINTF(("gzip_global: not enough data (%d)\n",
size));
return 0;
}
/* XXX this is pretty basic,
* needs to be expanded to ignore MTIME, OS,
* but still ensure flags are 0.
* Q. Do we need to support the flags and
* optional header fields? Likely.
* XXX add flag and field support too.
*/
if (memcmp(data, gzip_header, sizeof(gzip_header)) != 0) {
DPRINTF(("gzip_global: unsupported gzip header (%02x%02x)\n",
data[0], data[1]));
return 0;
} else {
DPRINTF(("gzip_global.%d: gzip header ok\n",__LINE__));
}
memcpy(&isize, &data[size-sizeof(uint32_t)], sizeof(uint32_t));
LE32TOH(isize);
memcpy(&icrc, &data[size-2*sizeof(uint32_t)], sizeof(uint32_t));
LE32TOH(icrc);
DPRINTF(("gzip_global: isize = %d (%02x %02x %02x %02x)\n",
isize,
data[size-4],
data[size-3],
data[size-2],
data[size-1]));
buf[0].size = isize;
crc = crc32(0, NULL, 0); /* get initial crc value */
/* skip over the gzip header */
zbuf.next_in = data + sizeof(gzip_header);
/* actual payload size stripped of gzip header and tail */
zbuf.avail_in = size - sizeof(gzip_header) - GZIP_TAIL_SIZE;
}
buf[0].out = malloc(buf[0].size, M_CRYPTO_DATA, M_NOWAIT);
if (buf[0].out == NULL)
return 0;
zbuf.next_out = buf[0].out;
zbuf.avail_out = buf[0].size;
DPRINTF(("zbuf avail_in %d, avail_out %d\n",
zbuf.avail_in, zbuf.avail_out));
i = 1;
error = decomp ? inflateInit2(&zbuf, window_inflate) :
deflateInit2(&zbuf, Z_DEFAULT_COMPRESSION, Z_METHOD,
window_deflate, Z_MEMLEVEL, Z_DEFAULT_STRATEGY);
if (error != Z_OK) {
printf("deflateInit2() failed\n");
goto bad2;
}
for (;;) {
DPRINTF(("pre: %s in:%d out:%d\n", decomp ? "deflate()" : "inflate()",
zbuf.avail_in, zbuf.avail_out));
error = decomp ? inflate(&zbuf, Z_SYNC_FLUSH) :
deflate(&zbuf, Z_FINISH);
DPRINTF(("post: %s in:%d out:%d\n", decomp ? "deflate()" : "inflate()",
zbuf.avail_in, zbuf.avail_out));
if (error == Z_STREAM_END) /* success */
break;
/*
* XXX compensate for a zlib problem:
* -sys/net/zlib.c has a bug which makes that Z_BUF_ERROR is
* set after successful decompression under rare conditions.
*/
else if (decomp && error == Z_BUF_ERROR
&& zbuf.avail_in == 0 && zbuf.avail_out != 0)
break;
else if (error != Z_OK)
goto bad;
else if (zbuf.avail_out == 0) {
/* we need more output space, allocate size */
int nextsize = buf[i-1].size * 2;
if (i == ZBUF || nextsize > 1000000)
goto bad;
buf[i].out = malloc(nextsize, M_CRYPTO_DATA, M_NOWAIT);
if (buf[i].out == NULL)
goto bad;
zbuf.next_out = buf[i].out;
zbuf.avail_out = buf[i].size = nextsize;
i++;
}
}
if (decomp) {
count = result = zbuf.total_out;
} else {
/* need room for header, CRC, and ISIZE */
result = zbuf.total_out + sizeof(gzip_header) + GZIP_TAIL_SIZE;
count = zbuf.total_out;
}
DPRINTF(("gzip_global: in %d -> out %d\n", size, result));
*out = malloc(result, M_CRYPTO_DATA, M_NOWAIT);
if (*out == NULL)
goto bad;
output = *out;
if (decomp)
inflateEnd(&zbuf);
else {
deflateEnd(&zbuf);
/* fill in gzip header */
memcpy(output, gzip_header, sizeof(gzip_header));
output += sizeof(gzip_header);
}
for (j = 0; j < i; j++) {
if (decomp) {
/* update crc for decompressed data */
crc = crc32(crc, buf[j].out, MIN(count, buf[j].size));
}
if (count > buf[j].size) {
memcpy(output, buf[j].out, buf[j].size);
output += buf[j].size;
free(buf[j].out, M_CRYPTO_DATA);
count -= buf[j].size;
} else {
/* it should be the last buffer */
memcpy(output, buf[j].out, count);
output += count;
free(buf[j].out, M_CRYPTO_DATA);
count = 0;
}
}
if (!decomp) {
/* fill in CRC and ISIZE */
HTOLE32(crc);
memcpy(output, &crc, sizeof(uint32_t));
HTOLE32(size);
memcpy(output + sizeof(uint32_t), &size, sizeof(uint32_t));
DPRINTF(("gzip_global: size = 0x%x (%02x %02x %02x %02x)\n",
size,
output[7],
output[3],
output[5],
output[4]));
} else {
if (crc != icrc || result != isize) {
DPRINTF(("gzip_global: crc/size mismatch\n"));
free(*out, M_CRYPTO_DATA);
*out = NULL;
return 0;
}
}
return result;
bad:
if (decomp)
inflateEnd(&zbuf);
else
deflateEnd(&zbuf);
bad2:
*out = NULL;
for (j = 0; j < i; j++)
free(buf[j].out, M_CRYPTO_DATA);
return 0;
}
signed ModuleCommit (struct plc * plc, uint32_t options)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_module_operation_commit_request
{
struct ethernet_std ethernet;
struct qualcomm_std qualcomm;
uint32_t RESERVED;
uint8_t NUM_OP_DATA;
struct __packed
{
uint16_t MOD_OP;
uint16_t MOD_OP_DATA_LEN;
uint32_t MOD_OP_RSVD;
uint32_t MOD_OP_SESSION_ID;
uint32_t COMMIT_CODE;
}
request;
uint8_t RSVD [20];
}
* request = (struct vs_module_operation_commit_request *)(message);
struct __packed vs_module_operation_commit_confirm
{
struct ethernet_std ethernet;
struct qualcomm_std qualcomm;
uint16_t MSTATUS;
uint16_t ERR_REC_CODE;
uint32_t RESERVED1;
uint8_t NUM_OP_DATA;
struct __packed
{
uint16_t MOD_OP;
uint16_t MOD_OP_DATA_LEN;
uint32_t MOD_OP_RSVD;
uint32_t MOD_OP_SESSION_ID;
uint32_t COMMIT_CODE;
uint8_t NUM_MODULES;
}
request;
struct __packed
{
uint16_t MOD_STATUS;
uint16_t ERR_REC_CODE;
}
MOD_OP_DATA [1];
}
* confirm = (struct vs_module_operation_commit_confirm *)(message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
unsigned timer = channel->timeout;
Request (plc, "Close Session");
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_MODULE_OPERATION | MMTYPE_REQ));
plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
request->NUM_OP_DATA = 1;
request->request.MOD_OP = HTOLE16 (PLC_MOD_OP_CLOSE_SESSION);
request->request.MOD_OP_DATA_LEN = HTOLE16 (sizeof (request->request) + sizeof (request->RSVD));
request->request.MOD_OP_SESSION_ID = HTOLE32 (plc->cookie);
request->request.COMMIT_CODE = HTOLE32 (options);
if (SendMME (plc) <= 0)
{
error ((plc->flags & PLC_BAILOUT), errno, CHANNEL_CANTSEND);
return (-1);
}
channel->timeout = PLC_MODULE_WRITE_TIMEOUT;
if (ReadMME (plc, 0, (VS_MODULE_OPERATION | MMTYPE_CNF)) <= 0)
{
error ((plc->flags & PLC_BAILOUT), errno, CHANNEL_CANTREAD);
channel->timeout = timer;
return (-1);
}
channel->timeout = timer;
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
return (-1);
}
return (0);
}
static signed psin (struct _file_ * pib)
{
unsigned index = 0;
unsigned count = 0;
unsigned limit = pibscalers (pib);
uint32_t value = 0;
signed c;
if ((limit != INT_CARRIERS) && (limit != AMP_CARRIERS) && (limit != PLC_CARRIERS))
{
error (1, 0, "Don't understand this PIB's prescaler format");
}
if (limit == INT_CARRIERS)
{
if (lseek (pib->file, INT_PRESCALER_OFFSET, SEEK_SET) != INT_PRESCALER_OFFSET)
{
error (1, errno, FILE_CANTSEEK, pib->name);
}
}
else if (limit == PLC_CARRIERS)
{
if (lseek (pib->file, QCA_PRESCALER_OFFSET, SEEK_SET) != QCA_PRESCALER_OFFSET)
{
error (1, errno, FILE_CANTSEEK, pib->name);
}
}
while ((c = getc (stdin)) != EOF)
{
if (isspace (c))
{
continue;
}
if ((c == '#') || (c == ';'))
{
do
{
c = getc (stdin);
}
while (nobreak (c));
continue;
}
index = 0;
while (isdigit (c))
{
index *= 10;
index += c - '0';
c = getc (stdin);
}
if (index != count)
{
error (1, ECANCELED, "Carrier %d out of order", index);
}
if (index >= limit)
{
error (1, EOVERFLOW, "Too many prescalers");
}
while (isblank (c))
{
c = getc (stdin);
}
value = 0;
while (isxdigit (c))
{
value *= 16;
value += todigit (c);
c = getc (stdin);
}
if (limit == INT_CARRIERS)
{
value = HTOLE32 (value);
if (write (pib->file, &value, sizeof (value)) != sizeof (value))
{
error (1, errno, "Can't save %s", pib->name);
}
}
else if (limit == AMP_CARRIERS)
{
if (value & ~0x03FF)
{
error (1, errno, "Position %d has invalid prescaler value", index);
}
if (ar7x00_psin (pib, value, index))
{
error (1, errno, "Can't update %s", pib->name);
}
}
else if (limit == PLC_CARRIERS)
{
uint8_t tmp = value & 0xff;
if (write (pib->file, &tmp, sizeof (tmp)) != sizeof (tmp))
{
error (1, errno, "Can't save %s", pib->name);
}
}
while (nobreak (c))
{
c = getc (stdin);
};
count++;
}
return (0);
}
int
ubsec_process(struct cryptop *crp)
{
struct ubsec_q *q = NULL;
int card, err = 0, i, j, s, nicealign;
struct ubsec_softc *sc;
struct cryptodesc *crd1, *crd2, *maccrd, *enccrd;
int encoffset = 0, macoffset = 0, cpskip, cpoffset;
int sskip, dskip, stheend, dtheend;
int16_t coffset;
struct ubsec_session *ses, key;
struct ubsec_dma *dmap = NULL;
u_int16_t flags = 0;
int ivlen = 0, keylen = 0;
if (crp == NULL || crp->crp_callback == NULL) {
ubsecstats.hst_invalid++;
return (EINVAL);
}
card = UBSEC_CARD(crp->crp_sid);
if (card >= ubsec_cd.cd_ndevs || ubsec_cd.cd_devs[card] == NULL) {
ubsecstats.hst_invalid++;
return (EINVAL);
}
sc = ubsec_cd.cd_devs[card];
s = splnet();
if (SIMPLEQ_EMPTY(&sc->sc_freequeue)) {
ubsecstats.hst_queuefull++;
splx(s);
err = ENOMEM;
goto errout2;
}
q = SIMPLEQ_FIRST(&sc->sc_freequeue);
SIMPLEQ_REMOVE_HEAD(&sc->sc_freequeue, q_next);
splx(s);
dmap = q->q_dma; /* Save dma pointer */
bzero(q, sizeof(struct ubsec_q));
bzero(&key, sizeof(key));
q->q_sesn = UBSEC_SESSION(crp->crp_sid);
q->q_dma = dmap;
ses = &sc->sc_sessions[q->q_sesn];
if (crp->crp_flags & CRYPTO_F_IMBUF) {
q->q_src_m = (struct mbuf *)crp->crp_buf;
q->q_dst_m = (struct mbuf *)crp->crp_buf;
} else if (crp->crp_flags & CRYPTO_F_IOV) {
q->q_src_io = (struct uio *)crp->crp_buf;
q->q_dst_io = (struct uio *)crp->crp_buf;
} else {
err = EINVAL;
goto errout; /* XXX we don't handle contiguous blocks! */
}
bzero(&dmap->d_dma->d_mcr, sizeof(struct ubsec_mcr));
dmap->d_dma->d_mcr.mcr_pkts = htole16(1);
dmap->d_dma->d_mcr.mcr_flags = 0;
q->q_crp = crp;
crd1 = crp->crp_desc;
if (crd1 == NULL) {
err = EINVAL;
goto errout;
}
crd2 = crd1->crd_next;
if (crd2 == NULL) {
if (crd1->crd_alg == CRYPTO_MD5_HMAC ||
crd1->crd_alg == CRYPTO_SHA1_HMAC) {
maccrd = crd1;
enccrd = NULL;
} else if (crd1->crd_alg == CRYPTO_3DES_CBC ||
crd1->crd_alg == CRYPTO_AES_CBC) {
maccrd = NULL;
enccrd = crd1;
} else {
err = EINVAL;
goto errout;
}
} else {
if ((crd1->crd_alg == CRYPTO_MD5_HMAC ||
crd1->crd_alg == CRYPTO_SHA1_HMAC) &&
(crd2->crd_alg == CRYPTO_3DES_CBC ||
crd2->crd_alg == CRYPTO_AES_CBC) &&
((crd2->crd_flags & CRD_F_ENCRYPT) == 0)) {
maccrd = crd1;
enccrd = crd2;
} else if ((crd1->crd_alg == CRYPTO_3DES_CBC ||
crd1->crd_alg == CRYPTO_AES_CBC) &&
(crd2->crd_alg == CRYPTO_MD5_HMAC ||
crd2->crd_alg == CRYPTO_SHA1_HMAC) &&
(crd1->crd_flags & CRD_F_ENCRYPT)) {
enccrd = crd1;
maccrd = crd2;
} else {
/*
* We cannot order the ubsec as requested
*/
err = EINVAL;
goto errout;
}
}
if (enccrd) {
if (enccrd->crd_alg == CRYPTO_AES_CBC) {
if ((sc->sc_flags & UBS_FLAGS_AES) == 0) {
err = EINVAL;
goto errout;
}
flags |= htole16(UBS_PKTCTX_ENC_AES);
switch (enccrd->crd_klen) {
case 128:
case 192:
case 256:
keylen = enccrd->crd_klen / 8;
break;
default:
err = EINVAL;
goto errout;
}
ivlen = 16;
} else {
flags |= htole16(UBS_PKTCTX_ENC_3DES);
ivlen = 8;
keylen = 24;
}
encoffset = enccrd->crd_skip;
if (enccrd->crd_flags & CRD_F_ENCRYPT) {
if (enccrd->crd_flags & CRD_F_IV_EXPLICIT)
bcopy(enccrd->crd_iv, key.ses_iv, ivlen);
else
arc4random_buf(key.ses_iv, ivlen);
if ((enccrd->crd_flags & CRD_F_IV_PRESENT) == 0) {
if (crp->crp_flags & CRYPTO_F_IMBUF)
err = m_copyback(q->q_src_m,
enccrd->crd_inject,
ivlen, key.ses_iv, M_NOWAIT);
else if (crp->crp_flags & CRYPTO_F_IOV)
cuio_copyback(q->q_src_io,
enccrd->crd_inject,
ivlen, key.ses_iv);
if (err)
goto errout;
}
} else {
flags |= htole16(UBS_PKTCTX_INBOUND);
if (enccrd->crd_flags & CRD_F_IV_EXPLICIT)
bcopy(enccrd->crd_iv, key.ses_iv, ivlen);
else if (crp->crp_flags & CRYPTO_F_IMBUF)
m_copydata(q->q_src_m, enccrd->crd_inject,
ivlen, (caddr_t)key.ses_iv);
else if (crp->crp_flags & CRYPTO_F_IOV)
cuio_copydata(q->q_src_io,
enccrd->crd_inject, ivlen,
(caddr_t)key.ses_iv);
}
for (i = 0; i < (keylen / 4); i++)
key.ses_key[i] = ses->ses_key[i];
for (i = 0; i < (ivlen / 4); i++)
SWAP32(key.ses_iv[i]);
}
if (maccrd) {
macoffset = maccrd->crd_skip;
if (maccrd->crd_alg == CRYPTO_MD5_HMAC)
flags |= htole16(UBS_PKTCTX_AUTH_MD5);
else
flags |= htole16(UBS_PKTCTX_AUTH_SHA1);
for (i = 0; i < 5; i++) {
key.ses_hminner[i] = ses->ses_hminner[i];
key.ses_hmouter[i] = ses->ses_hmouter[i];
HTOLE32(key.ses_hminner[i]);
HTOLE32(key.ses_hmouter[i]);
}
}
if (enccrd && maccrd) {
/*
* ubsec cannot handle packets where the end of encryption
* and authentication are not the same, or where the
* encrypted part begins before the authenticated part.
*/
if (((encoffset + enccrd->crd_len) !=
(macoffset + maccrd->crd_len)) ||
(enccrd->crd_skip < maccrd->crd_skip)) {
err = EINVAL;
goto errout;
}
sskip = maccrd->crd_skip;
cpskip = dskip = enccrd->crd_skip;
stheend = maccrd->crd_len;
dtheend = enccrd->crd_len;
coffset = enccrd->crd_skip - maccrd->crd_skip;
cpoffset = cpskip + dtheend;
#ifdef UBSEC_DEBUG
printf("mac: skip %d, len %d, inject %d\n",
maccrd->crd_skip, maccrd->crd_len, maccrd->crd_inject);
printf("enc: skip %d, len %d, inject %d\n",
enccrd->crd_skip, enccrd->crd_len, enccrd->crd_inject);
printf("src: skip %d, len %d\n", sskip, stheend);
printf("dst: skip %d, len %d\n", dskip, dtheend);
printf("ubs: coffset %d, pktlen %d, cpskip %d, cpoffset %d\n",
coffset, stheend, cpskip, cpoffset);
#endif
} else {
cpskip = dskip = sskip = macoffset + encoffset;
dtheend = stheend = (enccrd)?enccrd->crd_len:maccrd->crd_len;
cpoffset = cpskip + dtheend;
coffset = 0;
}
if (bus_dmamap_create(sc->sc_dmat, 0xfff0, UBS_MAX_SCATTER,
0xfff0, 0, BUS_DMA_NOWAIT, &q->q_src_map) != 0) {
err = ENOMEM;
goto errout;
}
if (crp->crp_flags & CRYPTO_F_IMBUF) {
if (bus_dmamap_load_mbuf(sc->sc_dmat, q->q_src_map,
q->q_src_m, BUS_DMA_NOWAIT) != 0) {
bus_dmamap_destroy(sc->sc_dmat, q->q_src_map);
q->q_src_map = NULL;
err = ENOMEM;
goto errout;
}
} else if (crp->crp_flags & CRYPTO_F_IOV) {
if (bus_dmamap_load_uio(sc->sc_dmat, q->q_src_map,
q->q_src_io, BUS_DMA_NOWAIT) != 0) {
bus_dmamap_destroy(sc->sc_dmat, q->q_src_map);
q->q_src_map = NULL;
err = ENOMEM;
goto errout;
}
}
nicealign = ubsec_dmamap_aligned(q->q_src_map);
dmap->d_dma->d_mcr.mcr_pktlen = htole16(stheend);
#ifdef UBSEC_DEBUG
printf("src skip: %d\n", sskip);
#endif
for (i = j = 0; i < q->q_src_map->dm_nsegs; i++) {
struct ubsec_pktbuf *pb;
bus_size_t packl = q->q_src_map->dm_segs[i].ds_len;
bus_addr_t packp = q->q_src_map->dm_segs[i].ds_addr;
if (sskip >= packl) {
sskip -= packl;
continue;
}
packl -= sskip;
packp += sskip;
sskip = 0;
if (packl > 0xfffc) {
err = EIO;
goto errout;
}
if (j == 0)
pb = &dmap->d_dma->d_mcr.mcr_ipktbuf;
else
pb = &dmap->d_dma->d_sbuf[j - 1];
pb->pb_addr = htole32(packp);
if (stheend) {
if (packl > stheend) {
pb->pb_len = htole32(stheend);
stheend = 0;
} else {
pb->pb_len = htole32(packl);
stheend -= packl;
}
} else
pb->pb_len = htole32(packl);
if ((i + 1) == q->q_src_map->dm_nsegs)
pb->pb_next = 0;
else
pb->pb_next = htole32(dmap->d_alloc.dma_paddr +
offsetof(struct ubsec_dmachunk, d_sbuf[j]));
j++;
}
if (enccrd == NULL && maccrd != NULL) {
dmap->d_dma->d_mcr.mcr_opktbuf.pb_addr = 0;
dmap->d_dma->d_mcr.mcr_opktbuf.pb_len = 0;
dmap->d_dma->d_mcr.mcr_opktbuf.pb_next =
htole32(dmap->d_alloc.dma_paddr +
offsetof(struct ubsec_dmachunk, d_macbuf[0]));
#ifdef UBSEC_DEBUG
printf("opkt: %x %x %x\n",
dmap->d_dma->d_mcr.mcr_opktbuf.pb_addr,
dmap->d_dma->d_mcr.mcr_opktbuf.pb_len,
dmap->d_dma->d_mcr.mcr_opktbuf.pb_next);
#endif
} else {
signed Identity2 (struct plc * plc)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_module_operation_read_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint32_t RESERVED;
uint8_t NUM_OP_DATA;
struct __packed
{
uint16_t MOD_OP;
uint16_t MOD_OP_DATA_LEN;
uint32_t MOD_OP_RSVD;
uint16_t MODULE_ID;
uint16_t MODULE_SUB_ID;
uint16_t MODULE_LENGTH;
uint32_t MODULE_OFFSET;
}
MODULE_SPEC;
}
* request = (struct vs_module_operation_read_request *)(message);
struct __packed vs_module_operation_read_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint16_t MSTATUS;
uint16_t ERR_REC_CODE;
uint32_t RESERVED;
uint8_t NUM_OP_DATA;
struct __packed
{
uint16_t MOD_OP;
uint16_t MOD_OP_DATA_LEN;
uint32_t MOD_OP_RSVD;
uint16_t MODULE_ID;
uint16_t MODULE_SUB_ID;
uint16_t MODULE_LENGTH;
uint32_t MODULE_OFFSET;
}
MODULE_SPEC;
uint8_t MODULE_DATA [PLC_MODULE_SIZE];
}
* confirm = (struct vs_module_operation_read_confirm *)(message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_MODULE_OPERATION | MMTYPE_REQ));
plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
request->NUM_OP_DATA = 1;
request->MODULE_SPEC.MOD_OP = HTOLE16 (0);
request->MODULE_SPEC.MOD_OP_DATA_LEN = HTOLE16 (sizeof (request->MODULE_SPEC));
request->MODULE_SPEC.MOD_OP_RSVD = HTOLE32 (0);
request->MODULE_SPEC.MODULE_ID = HTOLE16 (PLC_MODULEID_PARAMETERS);
request->MODULE_SPEC.MODULE_SUB_ID = HTOLE16 (0);
request->MODULE_SPEC.MODULE_LENGTH = HTOLE16 (PLC_MODULE_SIZE);
request->MODULE_SPEC.MODULE_OFFSET = HTOLE32 (0);
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
if (ReadMME (plc, 0, (VS_MODULE_OPERATION | MMTYPE_CNF)) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTREAD);
return (-1);
}
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
return (-1);
}
pibchain2 (confirm->MODULE_DATA, "device", plc->flags);
return (0);
}
signed CPLFirmware::Read (CPLChannel * channel)
{
ointellon intellon;
uint8_t message [ETHER_MAX_LEN];
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_rd_mod_request
{
struct header_eth ethernet;
struct header_int intellon;
uint8_t MODULEID;
uint8_t MACCESS;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint8_t MSECRET [16];
}
* request = (struct vs_rd_mod_request *) (message);
struct __packed vs_rd_mod_confirm
{
struct header_eth ethernet;
struct header_int intellon;
uint8_t MSTATUS;
uint8_t RES [3];
uint8_t MODULEID;
uint8_t RESERVED;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint32_t CHKSUM;
uint8_t BUFFER [INT6K_BLOCKSIZE];
}
* confirm = (struct vs_rd_mod_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
uint16_t extent = 0;
uint16_t length = oINTELLON_BLOCKSIZE;
uint32_t offset = 0;
intellon.ImportPeerAddress (channel->PeerAddress ());
intellon.ImportHostAddress (channel->HostAddress ());
intellon.SetMessageType (VS_RD_MOD | MMTYPE_REQ);
do
{
std::memset (message, 0, sizeof (message));
request = (struct vs_rd_mod_request *)(intellon.ExportHeader (message));
confirm = (struct vs_rd_mod_confirm *)(request);
request->MODULEID = VS_MODULE_MAC;
request->MLENGTH = HTOLE16 (length);
request->MOFFSET = HTOLE32 (offset);
if (channel->SendMessage (message, ETHER_MIN_LEN) <= 0)
{
oerror::error (0, ECANCELED, CPLCHANNEL_CANTSEND);
return (-1);
}
if (channel->ReadMessage (message, sizeof (message)) <= 0)
{
oerror::error (0, ECANCELED, CPLCHANNEL_CANTREAD);
return (-1);
}
if (confirm->MSTATUS)
{
oerror::error (0, ECANCELED, CPLCHANNEL_WONTDOIT);
return (-1);
}
#if 1
if (LE16TOH (confirm->MLENGTH) != length)
{
oerror::error (0, ECANCELED, oINTELLON_BAD_LENGTH);
return (-1);
}
if (LE32TOH (confirm->MOFFSET) != offset)
{
oerror::error (0, ECANCELED, oINTELLON_BAD_LENGTH);
return (-1);
}
#else
if (LE16TOH (confirm->MLENGTH) != length)
{
oerror::error (0, EIO, oINTELLON_BAD_LENGTH);
this->mlength = INT6K_BLOCKSIZE;
this->moffset = 0;
continue;
}
if (LE32TOH (confirm->MOFFSET) != this->moffset)
{
oerror::error (0, EIO, oINTELLON_BAD_OFFSET);
this->mlength = INT6K_BLOCKSIZE;
this->moffset = 0;
continue;
}
#endif
length = LE16TOH (confirm->MLENGTH);
offset = LE32TOH (confirm->MOFFSET);
if (omemory::checksum32 (confirm->MBUFFER, length, confirm->MCHKSUM))
{
oerror::error (0, ECANCELED, "Bad Message Checksum");
return (-1);
}
if (offset == extent)
{
CPLFirmware::Header * header = (CPLFirmware::Header *)(confirm->BUFFER);
if (checksum_32 (header, sizeof (CPLFirmware::Header), 0))
{
oerror::error (0, ECANCELED, "Bad Header Checksum");
return (-1);
}
if (header_nvm->HEADERVERSION != NVM_HEADER_VERSION)
{
oerror::error (0, ECANCELED, "Bad Header Version");
return (-1);
}
extent += sizeof (CPLFirmware::Header);
extent += LE32TOH (header_nvm->IMAGELENGTH);
header = LE16TOH (header_nvm->NEXTHEADER);
}
if ((offset + length) > extent)
{
length = extent - offset;
}
std::memcpy (this->mbuffer + offset, confirm->MBUFFER, length);
offset += length;
extent -= length;
}
while ((header) || (offset < extent));
return (0);
}
signed ReadFirmware1 (struct plc * plc)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_rd_mod_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MODULEID;
uint8_t MACCESS;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint8_t MSECRET [16];
}
* request = (struct vs_rd_mod_request *) (message);
struct __packed vs_rd_mod_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MSTATUS;
uint8_t RES [3];
uint8_t MODULEID;
uint8_t RESERVED;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint32_t CHKSUM;
uint8_t BUFFER [PLC_RECORD_SIZE];
}
* confirm = (struct vs_rd_mod_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
uint32_t header = 0;
uint32_t extent = 0;
uint32_t offset = 0;
uint16_t length = PLC_RECORD_SIZE;
Request (plc, "Read Firmware from Device");
if (lseek (plc->nvm.file, 0, SEEK_SET))
{
error (PLC_EXIT (plc), errno, FILE_CANTHOME, plc->nvm.name);
return (1);
}
do
{
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_RD_MOD | MMTYPE_REQ));
plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
request->MODULEID = VS_MODULE_MAC;
request->MLENGTH = HTOLE16 (length);
request->MOFFSET = HTOLE32 (offset);
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
if (ReadMME (plc, 0, (VS_RD_MOD | MMTYPE_CNF)) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTREAD);
return (-1);
}
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
return (-1);
}
if (LE16TOH (confirm->MLENGTH) != length)
{
error (PLC_EXIT (plc), 0, PLC_ERR_LENGTH);
return (-1);
}
if (LE32TOH (confirm->MOFFSET) != offset)
{
error (PLC_EXIT (plc), 0, PLC_ERR_OFFSET);
return (-1);
}
length = LE16TOH (confirm->MLENGTH);
offset = LE32TOH (confirm->MOFFSET);
if (checksum32 (confirm->BUFFER, length, confirm->CHKSUM))
{
error (PLC_EXIT (plc), ECANCELED, "Bad Packet Checksum");
return (-1);
}
if (offset == extent)
{
struct nvm_header1 * nvm_header = (struct nvm_header1 *)(confirm->BUFFER);
if (checksum32 (nvm_header, sizeof (* nvm_header), 0))
{
error (PLC_EXIT (plc), ECANCELED, "Bad Header Checksum");
return (-1);
}
if (LE32TOH (nvm_header->HEADERVERSION) != 0x60000000)
{
error (PLC_EXIT (plc), ECANCELED, "Bad Header Version");
return (-1);
}
extent += sizeof (* nvm_header);
extent += LE32TOH (nvm_header->IMAGELENGTH);
header = LE32TOH (nvm_header->NEXTHEADER);
}
if ((offset + length) > extent)
{
length = extent - offset;
}
if (lseek (plc->nvm.file, offset, SEEK_SET) != (off_t)(offset))
{
error (PLC_EXIT (plc), errno, FILE_CANTSEEK, plc->nvm.name);
return (-1);
}
if (write (plc->nvm.file, confirm->BUFFER, length) != (signed)(length))
{
error (PLC_EXIT (plc), errno, FILE_CANTSEEK, plc->nvm.name);
return (-1);
}
offset += length;
length = 1024;
}
while ((header) || (offset < extent));
Confirm (plc, "Read %s", plc->nvm.name);
return (0);
}
signed ReadParameters1 (struct plc * plc)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_rd_mod_request
{
struct ethernet_std ethernet;
struct qualcomm_std qualcomm;
uint8_t MODULEID;
uint8_t MACCESS;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint8_t MSECRET [16];
}
* request = (struct vs_rd_mod_request *) (message);
struct __packed vs_rd_mod_confirm
{
struct ethernet_std ethernet;
struct qualcomm_std qualcomm;
uint8_t MSTATUS;
uint8_t RESERVED1 [3];
uint8_t MODULEID;
uint8_t RESERVED;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint32_t CHKSUM;
uint8_t BUFFER [PLC_RECORD_SIZE];
}
* confirm = (struct vs_rd_mod_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
uint32_t extent = 0;
uint32_t offset = 0;
uint16_t length = PLC_RECORD_SIZE;
Request (plc, "Read Parameters from Device");
if (lseek (plc->pib.file, 0, SEEK_SET))
{
error ((plc->flags & PLC_BAILOUT), errno, FILE_CANTHOME, plc->pib.name);
return (1);
}
do
{
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_RD_MOD | MMTYPE_REQ));
plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
request->MODULEID = VS_MODULE_PIB;
request->MLENGTH = HTOLE16 (length);
request->MOFFSET = HTOLE32 (offset);
if (SendMME (plc) <= 0)
{
error ((plc->flags & PLC_BAILOUT), errno, CHANNEL_CANTSEND);
return (-1);
}
if (ReadMME (plc, 0, (VS_RD_MOD | MMTYPE_CNF)) <= 0)
{
error ((plc->flags & PLC_BAILOUT), errno, CHANNEL_CANTREAD);
return (-1);
}
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
return (-1);
}
if (LE16TOH (confirm->MLENGTH) != length)
{
error ((plc->flags & PLC_BAILOUT), 0, PLC_ERR_LENGTH);
return (-1);
}
if (LE32TOH (confirm->MOFFSET) != offset)
{
error ((plc->flags & PLC_BAILOUT), 0, PLC_ERR_OFFSET);
return (-1);
}
length = LE16TOH (confirm->MLENGTH);
offset = LE32TOH (confirm->MOFFSET);
if (checksum32 (confirm->BUFFER, length, confirm->CHKSUM))
{
error ((plc->flags & PLC_BAILOUT), ECANCELED, "Bad Packet Checksum");
return (-1);
}
if (offset == extent)
{
struct pib_header * pib_header = (struct pib_header *) (confirm->BUFFER);
extent = LE16TOH (pib_header->PIBLENGTH);
}
if ((offset + length) > extent)
{
length = extent - offset;
}
if (lseek (plc->pib.file, offset, SEEK_SET) != (signed)(offset))
{
error ((plc->flags & PLC_BAILOUT), errno, FILE_CANTSEEK, plc->pib.name);
return (-1);
}
if (write (plc->pib.file, confirm->BUFFER, length) != (signed)(length))
{
error ((plc->flags & PLC_BAILOUT), errno, FILE_CANTSAVE, plc->pib.name);
return (-1);
}
offset += length;
}
while (offset < extent);
Confirm (plc, "Read %s", plc->pib.name);
return (0);
}
signed WriteFirmware1 (struct plc * plc, unsigned module, const struct nvm_header1 * nvm_header)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_wr_mem_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint32_t MOFFSET;
uint32_t MLENGTH;
uint8_t BUFFER [PLC_RECORD_SIZE];
}
* request = (struct vs_wr_mem_request *) (message);
struct __packed vs_wr_mem_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MSTATUS;
uint32_t MOFFSET;
uint32_t MLENGTH;
}
* confirm = (struct vs_wr_mem_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
uint32_t length = PLC_RECORD_SIZE;
uint32_t offset = LE32TOH (nvm_header->IMAGEADDRESS);
uint32_t extent = LE32TOH (nvm_header->IMAGELENGTH);
Request (plc, "Write %s (%d) (%08X:%d)", plc->NVM.name, module, offset, extent);
while (extent)
{
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_WR_MEM | MMTYPE_REQ));
if (length > extent)
{
length = extent;
}
if (read (plc->NVM.file, request->BUFFER, length) != (signed)(length))
{
error (1, errno, FILE_CANTREAD, plc->NVM.name);
}
request->MLENGTH = HTOLE32 (length);
request->MOFFSET = HTOLE32 (offset);
plc->packetsize = sizeof (* request);
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
if (ReadMME (plc, 0, (VS_WR_MEM | MMTYPE_CNF)) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTREAD);
return (-1);
}
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
return (-1);
}
if (LE32TOH (confirm->MLENGTH) != length)
{
error (PLC_EXIT (plc), 0, PLC_ERR_LENGTH);
return (-1);
}
if (LE32TOH (confirm->MOFFSET) != offset)
{
error (PLC_EXIT (plc), 0, PLC_ERR_OFFSET);
return (-1);
}
offset += length;
extent -= length;
}
return (0);
}
signed VersionInfo2 (struct plc * plc)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_sw_ver_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint32_t COOKIE;
}
* request = (struct vs_sw_ver_request *) (message);
struct __packed vs_sw_ver_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MSTATUS;
uint8_t MDEVICE_CLASS;
uint8_t MVERLENGTH;
char MVERSION [254];
uint32_t IDENT;
uint32_t STEPPING_NUM;
uint32_t COOKIE;
uint32_t RSVD [6];
}
* confirm = (struct vs_sw_ver_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
Request (plc, "Request Version Information");
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_SW_VER | MMTYPE_REQ));
request->COOKIE = HTOLE32 (plc->cookie);
plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
while (ReadMME (plc, 0, (VS_SW_VER | MMTYPE_CNF)) > 0)
{
#if 0
fprintf (stderr, "DEVICE CLASS (%d)\n", LE32TOH (confirm->MDEVICE_CLASS));
fprintf (stderr, "VERSION LENGTH (%d)\n", LE32TOH (confirm->MVERLENGTH));
fprintf (stderr, "VERSION STRING [%s]\n", LE32TOH (confirm->MVERSION));
fprintf (stderr, "IDENT [0x%08X]\n", LE32TOH (confirm->IDENT));
fprintf (stderr, "STEPPING NUMBER [0x%08X]\n", LE32TOH (confirm->STEPPING_NUM));
fprintf (stderr, "COOKIE [0x%08X]\n", LE32TOH (confirm->COOKIE));
#endif
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
continue;
}
chipset (confirm);
Display (plc, "%s %s", chipsetname (confirm->MDEVICE_CLASS), confirm->MVERSION);
}
return (0);
}
signed Identity1 (struct plc * plc)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_rd_mod_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MODULEID;
uint8_t MACCESS;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint8_t MSECRET [16];
}
* request = (struct vs_rd_mod_request *) (message);
struct __packed vs_rd_mod_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MSTATUS;
uint8_t RES [3];
uint8_t MODULEID;
uint8_t RESERVED;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint32_t MCHKSUM;
uint8_t BUFFER [PLC_RECORD_SIZE];
}
* confirm = (struct vs_rd_mod_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
Request (plc, "Device Identity");
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_RD_MOD | MMTYPE_REQ));
plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
request->MODULEID = VS_MODULE_PIB;
request->MLENGTH = HTOLE16 (sizeof (confirm->BUFFER));
request->MOFFSET = HTOLE32 (0);
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
while (ReadMME (plc, 0, (VS_RD_MOD | MMTYPE_CNF)) > 0)
{
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
continue;
}
Confirm (plc, "-------");
pibpeek1 (confirm->BUFFER);
}
return (0);
}
static signed ReadVLANIDs (struct plc * plc, uint32_t offset, uint32_t length)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_forward_config_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t RESERVED1;
uint8_t MREQUEST;
uint8_t MVERSION;
uint32_t RESERVED2;
uint32_t DATA_LENGTH;
uint32_t DATA_OFFSET;
uint16_t RESERVED3;
}
* request = (struct vs_forward_config_request *) (message);
struct __packed vs_forward_config_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t RESERVED1;
uint8_t RESULTCODE;
uint8_t OPERATION;
uint8_t MVERSION;
uint32_t RESERVED2;
uint32_t DATA_LENGTH;
uint32_t DATA_OFFSET;
uint8_t DATA [PLC_MODULE_SIZE];
}
* confirm = (struct vs_forward_config_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_FORWARD_CONFIG | MMTYPE_REQ));
request->MREQUEST = PLCFWD_GET;
request->MVERSION = PLCFWD_VER;
request->DATA_OFFSET = HTOLE32 (offset);
request->DATA_LENGTH = HTOLE32 (length);
plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
while (ReadMME (plc, 0, (VS_FORWARD_CONFIG | MMTYPE_CNF)) > 0)
{
if (confirm->RESULTCODE)
{
Failure (plc, PLC_WONTDOIT);
continue;
}
hexview (confirm->DATA, LE32TOH (confirm->DATA_OFFSET), LE32TOH (confirm->DATA_LENGTH), stdout);
}
return (0);
}
signed WriteMOD (struct plc * plc, uint8_t module, void const * memory, size_t extent)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_wr_mod_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MODULEID;
uint8_t MACCESS;
uint16_t MLENGTH;
uint32_t MOFFSET;
uint32_t MCHKSUM;
uint8_t MBUFFER [PLC_RECORD_SIZE];
}
* request = (struct vs_wr_mod_request *) (message);
struct __packed vs_wr_mod_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint8_t MSTATUS;
uint8_t MODULEID;
uint8_t MACCESS;
uint16_t MLENGTH;
uint32_t MOFFSET;
}
* confirm = (struct vs_wr_mod_confirm *) (message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
uint32_t length = PLC_RECORD_SIZE;
uint32_t offset = 0;
while (extent)
{
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_WR_MOD | MMTYPE_REQ));
if (length > extent)
{
length = (signed)(extent);
}
memcpy (request->MBUFFER, (byte *)(memory) + offset, length);
request->MODULEID = module;
request->MACCESS = 0;
request->MLENGTH = HTOLE16 (length);
request->MOFFSET = HTOLE32 (offset);
request->MCHKSUM = checksum32 (request->MBUFFER, length, 0);
plc->packetsize = sizeof (* request);
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
if (ReadMME (plc, 0, (VS_WR_MOD | MMTYPE_CNF)) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTREAD);
return (-1);
}
if (confirm->MSTATUS)
{
Failure (plc, PLC_WONTDOIT);
return (-1);
}
if (LE16TOH (confirm->MLENGTH) != length)
{
error (PLC_EXIT (plc), 0, PLC_ERR_LENGTH);
return (-1);
}
if (LE32TOH (confirm->MOFFSET) != offset)
{
error (PLC_EXIT (plc), 0, PLC_ERR_OFFSET);
return (-1);
}
offset += length;
extent -= length;
}
return (0);
}
signed ModuleRead (struct plc * plc, struct _file_ * file, uint16_t source, uint16_t module, uint16_t submodule)
{
struct channel * channel = (struct channel *)(plc->channel);
struct message * message = (struct message *)(plc->message);
#ifndef __GNUC__
#pragma pack (push,1)
#endif
struct __packed vs_module_operation_read_request
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint32_t RESERVED;
uint8_t NUM_OP_DATA;
struct __packed
{
uint16_t MOD_OP;
uint16_t MOD_OP_DATA_LEN;
uint32_t MOD_OP_RSVD;
uint16_t MODULE_ID;
uint16_t MODULE_SUB_ID;
uint16_t MODULE_LENGTH;
uint32_t MODULE_OFFSET;
}
MODULE_SPEC;
}
* request = (struct vs_module_operation_read_request *)(message);
struct __packed vs_module_operation_read_confirm
{
struct ethernet_hdr ethernet;
struct qualcomm_hdr qualcomm;
uint16_t MSTATUS;
uint16_t ERR_REC_CODE;
uint32_t RESERVED;
uint8_t NUM_OP_DATA;
struct __packed
{
uint16_t MOD_OP;
uint16_t MOD_OP_DATA_LEN;
uint32_t MOD_OP_RSVD;
uint16_t MODULE_ID;
uint16_t MODULE_SUB_ID;
uint16_t MODULE_LENGTH;
uint32_t MODULE_OFFSET;
}
MODULE_SPEC;
uint8_t MODULE_DATA [PLC_MODULE_SIZE];
}
* confirm = (struct vs_module_operation_read_confirm *)(message);
#ifndef __GNUC__
#pragma pack (pop)
#endif
unsigned timer = channel->timeout;
uint16_t length = PLC_MODULE_SIZE;
uint32_t offset = 0;
Request (plc, "Read Module from %s", source? "Flash": "Memory");
if (lseek (file->file, 0, SEEK_SET) == -1)
{
error (PLC_EXIT (plc), errno, FILE_CANTHOME, file->name);
return (-1);
}
while (length == PLC_MODULE_SIZE)
{
memset (message, 0, sizeof (* message));
EthernetHeader (&request->ethernet, channel->peer, channel->host, channel->type);
QualcommHeader (&request->qualcomm, 0, (VS_MODULE_OPERATION | MMTYPE_REQ));
plc->packetsize = (ETHER_MIN_LEN - ETHER_CRC_LEN);
request->NUM_OP_DATA = 1;
request->MODULE_SPEC.MOD_OP = HTOLE16 (source);
request->MODULE_SPEC.MOD_OP_DATA_LEN = HTOLE16 (sizeof (request->MODULE_SPEC));
request->MODULE_SPEC.MOD_OP_RSVD = HTOLE32 (0);
request->MODULE_SPEC.MODULE_ID = HTOLE16 (module);
request->MODULE_SPEC.MODULE_SUB_ID = HTOLE16 (submodule);
request->MODULE_SPEC.MODULE_LENGTH = HTOLE16 (length);
request->MODULE_SPEC.MODULE_OFFSET = HTOLE32 (offset);
#if 0
#if defined (__GNUC__)
#warning "Debug code active in module ModuleRead"
#endif
fprintf (stderr, "----- \n");
fprintf (stderr, "RESERVED 0x%08X\n", LE32TOH (request->RESERVED));
fprintf (stderr, "NUM_OP_DATA %d\n", request->NUM_OP_DATA);
fprintf (stderr, "MOD_OP 0x%02X\n", LE16TOH (request->MODULE_SPEC.MOD_OP));
fprintf (stderr, "MOD_OP_DATA_LEN %d\n", LE16TOH (request->MODULE_SPEC.MOD_OP_DATA_LEN));
fprintf (stderr, "RESERVED 0x%08X\n", LE32TOH (request->MODULE_SPEC.MOD_OP_RSVD));
fprintf (stderr, "MODULE_ID 0x%04X\n", LE16TOH (request->MODULE_SPEC.MODULE_ID));
fprintf (stderr, "MODULE_SUB_ID 0x%04X\n", LE16TOH (request->MODULE_SPEC.MODULE_SUB_ID));
fprintf (stderr, "MODULE_LENGTH %d\n", LE16TOH (request->MODULE_SPEC.MODULE_LENGTH));
fprintf (stderr, "MODULE_OFFSET 0x%08X\n", LE32TOH (request->MODULE_SPEC.MODULE_OFFSET));
fprintf (stderr, "\n");
#endif
if (SendMME (plc) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTSEND);
return (-1);
}
channel->timeout = PLC_MODULE_READ_TIMEOUT;
if (ReadMME (plc, 0, (VS_MODULE_OPERATION | MMTYPE_CNF)) <= 0)
{
error (PLC_EXIT (plc), errno, CHANNEL_CANTREAD);
return (-1);
}
channel->timeout = timer;
#if 0
#if defined (__GNUC__)
#warning "Debug code active in module ModuleRead"
#endif
fprintf (stderr, "MSTATUS 0x%04X\n", LE16TOH (confirm->MSTATUS));
fprintf (stderr, "ERROR_REC_CODE %d\n", LE16TOH (confirm->ERR_REC_CODE));
fprintf (stderr, "RESERVED 0x%08X\n", LE32TOH (confirm->RESERVED));
fprintf (stderr, "NUM_OP_DATA %d\n", confirm->NUM_OP_DATA);
fprintf (stderr, "MOD_OP 0x%02X\n", LE16TOH (confirm->MODULE_SPEC.MOD_OP));
fprintf (stderr, "MOD_OP_DATA_LEN %d\n", LE16TOH (confirm->MODULE_SPEC.MOD_OP_DATA_LEN));
fprintf (stderr, "RESERVED 0x%08X\n", LE32TOH (confirm->MODULE_SPEC.MOD_OP_RSVD));
fprintf (stderr, "MODULE_ID 0x%04X\n", LE16TOH (confirm->MODULE_SPEC.MODULE_ID));
fprintf (stderr, "MODULE_SUB_ID 0x%04X\n", LE16TOH (confirm->MODULE_SPEC.MODULE_SUB_ID));
fprintf (stderr, "MODULE_LENGTH %d\n", LE16TOH (confirm->MODULE_SPEC.MODULE_LENGTH));
fprintf (stderr, "MODULE_OFFSET 0x%08X\n", LE32TOH (confirm->MODULE_SPEC.MODULE_OFFSET));
fprintf (stderr, "\n");
#endif
if (LE16TOH (confirm->MSTATUS))
{
Failure (plc, PLC_WONTDOIT);
return (-1);
}
length = LE16TOH (confirm->MODULE_SPEC.MODULE_LENGTH);
offset = LE32TOH (confirm->MODULE_SPEC.MODULE_OFFSET);
if (write (file->file, confirm->MODULE_DATA, length) != (signed)(length))
{
error (PLC_EXIT (plc), errno, FILE_CANTSAVE, file->name);
return (-1);
}
offset += length;
}
return (0);
}