/**
* Send a Modbus write multiple registers command.
*
* @param modbus Previously initialized Modbus device structure.
* @param address The Modbus address of the first register to write.
* @param nb_registers The number of registers to write.
* @param registers Buffer holding all the registers values to write.
*
* @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
* SR_ERR_DATA upon invalid data, or SR_ERR on failure.
*/
SR_PRIV int sr_modbus_write_multiple_registers(struct sr_modbus_dev_inst*modbus,
int address, int nb_registers, uint16_t *registers)
{
uint8_t request[6 + (2 * nb_registers)], reply[5];
int ret;
if (address < 0 || address > 0xFFFF
|| nb_registers < 1 || nb_registers > 123 || !registers)
return SR_ERR_ARG;
W8(request + 0, MODBUS_WRITE_MULTIPLE_REGISTERS);
WB16(request + 1, address);
WB16(request + 3, nb_registers);
W8(request + 5, 2 * nb_registers);
memcpy(request + 6, registers, 2 * nb_registers);
ret = sr_modbus_request_reply(modbus, request, sizeof(request),
reply, sizeof(reply));
if (ret != SR_OK)
return ret;
if (sr_modbus_error_check(reply))
return SR_ERR_DATA;
if (memcmp(request, reply, sizeof(reply)))
return SR_ERR_DATA;
return SR_OK;
}
/**
* Send a Modbus read holding registers command and receive the corresponding
* registers values.
*
* @param modbus Previously initialized Modbus device structure.
* @param address The Modbus address of the first register to read, or -1 to
* read the reply of a previouly sent read registers command.
* @param nb_registers The number of registers to read.
* @param registers Buffer to store all the received registers values,
* or NULL to send the read holding registers command
* without reading the reply.
*
* @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
* SR_ERR_DATA upon invalid data, or SR_ERR on failure.
*/
SR_PRIV int sr_modbus_read_holding_registers(struct sr_modbus_dev_inst *modbus,
int address, int nb_registers, uint16_t *registers)
{
uint8_t request[5], reply[2 + (2 * nb_registers)];
int ret;
if (address < -1 || address > 0xFFFF
|| nb_registers < 1 || nb_registers > 125)
return SR_ERR_ARG;
W8(request + 0, MODBUS_READ_HOLDING_REGISTERS);
WB16(request + 1, address);
WB16(request + 3, nb_registers);
if (address >= 0) {
ret = sr_modbus_request(modbus, request, sizeof(request));
if (ret != SR_OK)
return ret;
}
if (registers) {
ret = sr_modbus_reply(modbus, reply, sizeof(reply));
if (ret != SR_OK)
return ret;
if (sr_modbus_error_check(reply))
return SR_ERR_DATA;
if (reply[0] != request[0] || R8(reply + 1) != (uint8_t)(2 * nb_registers))
return SR_ERR_DATA;
memcpy(registers, reply + 2, 2 * nb_registers);
}
return SR_OK;
}
/**
* Send a Modbus read coils command and receive the corresponding coils values.
*
* @param modbus Previously initialized Modbus device structure.
* @param address The Modbus address of the first coil to read, or -1 to read
* the reply of a previouly sent read coils command.
* @param nb_coils The number of coils to read.
* @param coils Buffer to store all the received coils values (1 bit per coil),
* or NULL to send the read coil command without reading the reply.
*
* @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
* SR_ERR_DATA upon invalid data, or SR_ERR on failure.
*/
SR_PRIV int sr_modbus_read_coils(struct sr_modbus_dev_inst *modbus,
int address, int nb_coils, uint8_t *coils)
{
uint8_t request[5], reply[2 + (nb_coils + 7) / 8];
int ret;
if (address < -1 || address > 0xFFFF || nb_coils < 1 || nb_coils > 2000)
return SR_ERR_ARG;
W8(request + 0, MODBUS_READ_COILS);
WB16(request + 1, address);
WB16(request + 3, nb_coils);
if (address >= 0) {
ret = sr_modbus_request(modbus, request, sizeof(request));
if (ret != SR_OK)
return ret;
}
if (coils) {
ret = sr_modbus_reply(modbus, reply, sizeof(reply));
if (ret != SR_OK)
return ret;
if (sr_modbus_error_check(reply))
return SR_ERR_DATA;
if (reply[0] != request[0] || R8(reply + 1) != (uint8_t)((nb_coils + 7) / 8))
return SR_ERR_DATA;
memcpy(coils, reply + 2, (nb_coils + 7) / 8);
}
return SR_OK;
}
void Io::ClearOthers ()
{
// FIXME !! shouldn't we call Write*() ?
// lcd
for (uint8_t i = 0x0; i < 0x56; i += 2)
Write16(i, 0x0000);
// dma
for (uint8_t i = 0xB0; i < 0xE0; i += 4)
Write32(i, 0x0000);
// FIXME : should timers be set to 0 too ? (vba does not)
W8(HALTCNT, 0xFF); // normal mode (internal)
W16(IE, 0x0000);
W16(IF, 0x0000);
W16(IME, 0x0000);
Write16(WAITCNT, 0x0000);
W16(BG2PA, 0x0100);
W16(BG2PD, 0x0100);
W16(BG3PA, 0x0100);
W16(BG3PD, 0x0100);
}
static void usbtmc_bulk_out_header_write(void *header, uint8_t MsgID,
uint8_t bTag,
uint32_t TransferSize,
uint8_t bmTransferAttributes,
char TermChar)
{
W8(header+ 0, MsgID);
W8(header+ 1, bTag);
W8(header+ 2, ~bTag);
W8(header+ 3, 0);
WL32(header+ 4, TransferSize);
W8(header+ 8, bmTransferAttributes);
W8(header+ 9, TermChar);
WL16(header+10, 0);
}
void Io::Reset ()
{
std::memset(m_iomem, 0, IO_SIZE);
// TODO use clears intead
// TODO DISPCNT should be 0x80 by default
// TODO do it also for clears
// TODO lcd should draw white lines when hblank forced
// TODO when passing disabling hblank forced, lcd should start drawing from
// first line
W16(SOUNDBIAS, 0x0200); // default value
W16(KEYINPUT, 0x03FF); // all keys released
W8(HALTCNT, 0xFF); // normal mode (internal)
W16(DISPSTAT, 0x0004); // vcount match (since default vcount is 0)
W16(BG2PA, 0x0100);
W16(BG2PD, 0x0100);
W16(BG3PA, 0x0100);
W16(BG3PD, 0x0100);
W16(RCNT, 0x8000); // we start in general purpose mode
}
/**
* Send a Modbus write coil command.
*
* @param modbus Previously initialized Modbus device structure.
* @param address The Modbus address of the coil to write.
* @param value The new value to assign to this coil.
*
* @return SR_OK upon success, SR_ERR_ARG upon invalid arguments,
* SR_ERR_DATA upon invalid data, or SR_ERR on failure.
*/
SR_PRIV int sr_modbus_write_coil(struct sr_modbus_dev_inst *modbus,
int address, int value)
{
uint8_t request[5], reply[5];
int ret;
if (address < 0 || address > 0xFFFF)
return SR_ERR_ARG;
W8(request + 0, MODBUS_WRITE_COIL);
WB16(request + 1, address);
WB16(request + 3, value ? 0xFF00 : 0);
ret = sr_modbus_request_reply(modbus, request, sizeof(request),
reply, sizeof(reply));
if (ret != SR_OK)
return ret;
if (sr_modbus_error_check(reply))
return SR_ERR_DATA;
if (memcmp(request, reply, sizeof(reply)))
return SR_ERR_DATA;
return SR_OK;
}
/*
* This function is called to start or restart the FEC during a link
* change. This only happens when switching between half and full
* duplex.
*/
static void restart(struct net_device *dev)
{
struct fs_enet_private *fep = netdev_priv(dev);
scc_t __iomem *sccp = fep->scc.sccp;
scc_enet_t __iomem *ep = fep->scc.ep;
const struct fs_platform_info *fpi = fep->fpi;
u16 paddrh, paddrm, paddrl;
const unsigned char *mac;
int i;
C32(sccp, scc_gsmrl, SCC_GSMRL_ENR | SCC_GSMRL_ENT);
/* clear everything (slow & steady does it) */
for (i = 0; i < sizeof(*ep); i++)
__fs_out8((u8 __iomem *)ep + i, 0);
/* point to bds */
W16(ep, sen_genscc.scc_rbase, fep->ring_mem_addr);
W16(ep, sen_genscc.scc_tbase,
fep->ring_mem_addr + sizeof(cbd_t) * fpi->rx_ring);
/* Initialize function code registers for big-endian.
*/
#ifndef CONFIG_NOT_COHERENT_CACHE
W8(ep, sen_genscc.scc_rfcr, SCC_EB | SCC_GBL);
W8(ep, sen_genscc.scc_tfcr, SCC_EB | SCC_GBL);
#else
W8(ep, sen_genscc.scc_rfcr, SCC_EB);
W8(ep, sen_genscc.scc_tfcr, SCC_EB);
#endif
/* Set maximum bytes per receive buffer.
* This appears to be an Ethernet frame size, not the buffer
* fragment size. It must be a multiple of four.
*/
W16(ep, sen_genscc.scc_mrblr, 0x5f0);
/* Set CRC preset and mask.
*/
W32(ep, sen_cpres, 0xffffffff);
W32(ep, sen_cmask, 0xdebb20e3);
W32(ep, sen_crcec, 0); /* CRC Error counter */
W32(ep, sen_alec, 0); /* alignment error counter */
W32(ep, sen_disfc, 0); /* discard frame counter */
W16(ep, sen_pads, 0x8888); /* Tx short frame pad character */
W16(ep, sen_retlim, 15); /* Retry limit threshold */
W16(ep, sen_maxflr, 0x5ee); /* maximum frame length register */
W16(ep, sen_minflr, PKT_MINBUF_SIZE); /* minimum frame length register */
W16(ep, sen_maxd1, 0x000005f0); /* maximum DMA1 length */
W16(ep, sen_maxd2, 0x000005f0); /* maximum DMA2 length */
/* Clear hash tables.
*/
W16(ep, sen_gaddr1, 0);
W16(ep, sen_gaddr2, 0);
W16(ep, sen_gaddr3, 0);
W16(ep, sen_gaddr4, 0);
W16(ep, sen_iaddr1, 0);
W16(ep, sen_iaddr2, 0);
W16(ep, sen_iaddr3, 0);
W16(ep, sen_iaddr4, 0);
/* set address
*/
mac = dev->dev_addr;
paddrh = ((u16) mac[5] << 8) | mac[4];
paddrm = ((u16) mac[3] << 8) | mac[2];
paddrl = ((u16) mac[1] << 8) | mac[0];
W16(ep, sen_paddrh, paddrh);
W16(ep, sen_paddrm, paddrm);
W16(ep, sen_paddrl, paddrl);
W16(ep, sen_pper, 0);
W16(ep, sen_taddrl, 0);
W16(ep, sen_taddrm, 0);
W16(ep, sen_taddrh, 0);
fs_init_bds(dev);
scc_cr_cmd(fep, CPM_CR_INIT_TRX);
W16(sccp, scc_scce, 0xffff);
/* Enable interrupts we wish to service.
*/
W16(sccp, scc_sccm, SCCE_ENET_TXE | SCCE_ENET_RXF | SCCE_ENET_TXB);
/* Set GSMR_H to enable all normal operating modes.
* Set GSMR_L to enable Ethernet to MC68160.
*/
W32(sccp, scc_gsmrh, 0);
W32(sccp, scc_gsmrl,
SCC_GSMRL_TCI | SCC_GSMRL_TPL_48 | SCC_GSMRL_TPP_10 |
SCC_GSMRL_MODE_ENET);
/* Set sync/delimiters.
*/
W16(sccp, scc_dsr, 0xd555);
/* Set processing mode. Use Ethernet CRC, catch broadcast, and
* start frame search 22 bit times after RENA.
*/
W16(sccp, scc_psmr, SCC_PSMR_ENCRC | SCC_PSMR_NIB22);
/* Set full duplex mode if needed */
if (fep->phydev->duplex)
S16(sccp, scc_psmr, SCC_PSMR_LPB | SCC_PSMR_FDE);
S32(sccp, scc_gsmrl, SCC_GSMRL_ENR | SCC_GSMRL_ENT);
}
void Io::Write8 (uint32_t add, uint8_t val)
{
//debug ("IO Write8 at " << IOS_ADD << add << " of " << IOS_ADD << (int)val);
switch (add & 0xFFF)
{
case NR10+1:
case NR52+1:
case NR52+2:
case NR52+3:
break;
case NR10:
case NR11:
case NR13:
case NR21:
case NR23:
case NR41:
case NR43:
case NR50:
case NR51:
case SOUNDCNT_H:
W8(add, val);
break;
case NR12:
W8(add, val);
if (!(val & (0xF << 4)))
SOUND.ResetSound1Envelope();
break;
case NR14:
W8(add, val & 0xC7);
if (val & (0x1 << 7))
SOUND.ResetSound1();
break;
case NR22:
W8(add, val);
if (!(val & (0xF << 4)))
SOUND.ResetSound2Envelope();
break;
case NR24:
W8(add, val & 0xC7);
if (val & (0x1 << 7))
SOUND.ResetSound2();
break;
case NR42:
W8(add, val);
if (!(val & (0xF << 4)))
SOUND.ResetSound4Envelope();
break;
case NR44:
W8(add, val & 0xC7);
if (val & (0x1 << 7))
SOUND.ResetSound4();
break;
case SOUNDCNT_H+1:
W8(add, val);
SOUND.UpdateCntH1(val);
break;
case NR52:
// this will also reset the sound on flags
W8(add, val & 0x80);
break;
case POSTFLG:
// FIXME is this right, i have no idea about why i should do that
if (val)
val &= 0xFE;
W8(add, val);
break;
case HALTCNT:
W8(add, val);
break;
default:
//W8(add, val);
// TODO make a function which will apply masks to IO memory and trigger
// the update functions, this function will be called by write8 and
// write16
#if 1
add &= 0xFFF;
if (add % 2)
Write16(add & ~0x1, (val << 8) | m_iomem[add & ~0x1]);
else
Write16(add, (m_iomem[add | 0x1] << 8) | val);
#endif
break;
}
}
