eModbus/src/CoilData.cpp at master · eModbus/eModbus · GitHub

History

556 lines (507 loc) · 15 KB

Raw

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

257

258

259

260

261

262

263

264

265

266

267

268

269

270

271

272

273

274

275

276

277

278

279

280

281

282

283

284

285

286

287

288

289

290

291

292

293

294

295

296

297

298

299

300

301

302

303

304

305

306

307

308

309

310

311

312

313

314

315

316

317

318

319

320

321

322

323

324

325

326

327

328

329

330

331

332

333

334

335

336

337

338

339

340

341

342

343

344

345

346

347

348

349

350

351

352

353

354

355

356

357

358

359

360

361

362

363

364

365

366

367

368

369

370

371

372

373

374

375

376

377

378

379

380

381

382

383

384

385

386

387

388

389

390

391

392

393

394

395

396

397

398

399

400

401

402

403

404

405

406

407

408

409

410

411

412

413

414

415

416

417

418

419

420

421

422

423

424

425

426

427

428

429

430

431

432

433

434

435

436

437

438

439

440

441

442

443

444

445

446

447

448

449

450

451

452

453

454

455

456

457

458

459

460

461

462

463

464

465

466

467

468

469

470

471

472

473

474

475

476

477

478

479

480

481

482

483

484

485

486

487

488

489

490

491

492

493

494

495

496

497

498

499

500

501

502

503

504

505

506

507

508

509

510

511

512

513

514

515

516

517

518

519

520

521

522

523

524

525

526

527

528

529

530

531

532

533

534

535

536

537

538

539

540

541

542

543

544

545

546

547

548

549

550

551

552

553

554

555

556

// =================================================================================================

// MIT license - see license.md for details

// =================================================================================================

#include "CoilData.h"

#undef LOCAL_LOG_LEVEL

#include "Logging.h"

// Constructor: optional size in bits, optional initial value for all bits

// Maximum size is 2000 coils (=250 bytes)

CoilData::CoilData(uint16_t size, bool initValue) :

CDsize(0),

CDbyteSize(0),

CDbuffer(nullptr) {

// Limit the size to 2000 (Modbus rules)

if (size > 2000) size = 2000;

// Do we have a size?

if (size) {

// Calculate number of bytes needed

CDbyteSize = byteIndex(size - 1) + 1;

// Allocate and init buffer

CDbuffer = new uint8_t[CDbyteSize];

memset(CDbuffer, initValue ? 0xFF : 0, CDbyteSize);

if (initValue) {

CDbuffer[CDbyteSize - 1] &= CDfilter[bitIndex(size - 1)];

}

CDsize = size;

}

// Alternate constructor, taking a "1101..." bit image char array to init

CoilData::CoilData(const char *initVector) :

CDsize(0),

CDbyteSize(0),

CDbuffer(nullptr) {

// Init with bit image array.

setVector(initVector);

}

// Destructor: take care of cleaning up

CoilData::~CoilData() {

if (CDbuffer) {

delete CDbuffer;

}

// Assignment operator

CoilData& CoilData::operator=(const CoilData& m) {

// Avoid self-assignment

if (this == &m) return *this;

// Remove old data

if (CDbuffer) {

delete CDbuffer;

}

// Are coils in source?

if (m.CDsize > 0) {

// Yes. Allocate new buffer and copy data

CDbuffer = new uint8_t[m.CDbyteSize];

memcpy(CDbuffer, m.CDbuffer, m.CDbyteSize);

CDsize = m.CDsize;

CDbyteSize = m.CDbyteSize;

} else {

// No, leave buffer empty

CDsize = 0;

CDbyteSize = 0;

CDbuffer = nullptr;

}

return *this;

}

// Copy constructor

CoilData::CoilData(const CoilData& m) :

CDsize(0),

CDbyteSize(0),

CDbuffer(nullptr) {

// Has the source coils at all?

if (m.CDsize > 0) {

// Yes. Allocate new buffer and copy data

CDbuffer = new uint8_t[m.CDbyteSize];

memcpy(CDbuffer, m.CDbuffer, m.CDbyteSize);

CDsize = m.CDsize;

CDbyteSize = m.CDbyteSize;

}

#ifndef NO_MOVE

// Move constructor

CoilData::CoilData(CoilData&& m) {

// Copy all data

CDbuffer = m.CDbuffer;

CDsize = m.CDsize;

CDbyteSize = m.CDbyteSize;

// Then clear source

m.CDbuffer = nullptr;

m.CDsize = 0;

m.CDbyteSize = 0;

}

// Move assignment

CoilData& CoilData::operator=(CoilData&& m) {

// Remove buffer, if already allocated

if (CDbuffer) {

delete CDbuffer;

}

// Are there coils in the source at all?

if (m.CDsize > 0) {

// Yes. Copy over all data

CDbuffer = m.CDbuffer;

CDsize = m.CDsize;

CDbyteSize = m.CDbyteSize;

// Then clear source

m.CDbuffer = nullptr;

m.CDsize = 0;

m.CDbyteSize = 0;

} else {

// No, leave object empty.

CDbuffer = nullptr;

CDsize = 0;

CDbyteSize = 0;

}

return *this;

}

#endif

// Comparison operators

bool CoilData::operator==(const CoilData& m) {

// Self-compare is always true

if (this == &m) return true;

// Different sizes are never equal

if (CDsize != m.CDsize) return false;

// Compare the data

if (CDsize > 0 && memcmp(CDbuffer, m.CDbuffer, CDbyteSize)) return false;

return true;

}

// Inequality: invert the result of the equality comparison

bool CoilData::operator!=(const CoilData& m) {

return !(*this == m);

}

// Assignment of a bit image char array to re-init

CoilData& CoilData::operator=(const char *initVector) {

// setVector() may be unsuccessful - then data is deleted!

setVector(initVector);

return *this;

}

// If used as vector<uint8_t>, return a complete slice

CoilData::operator vector<uint8_t> const () {

// Create new vector to return

vector<uint8_t> retval;

if (CDsize > 0) {

// Copy over all buffer content

retval.assign(CDbuffer, CDbuffer + CDbyteSize);

}

// return the copy (or an empty vector)

return retval;

}

// slice: return a CoilData object with coils shifted leftmost

// will return empty object if illegal parameters are detected

CoilData CoilData::slice(uint16_t start, uint16_t length) {

CoilData retval;

// Any slice of an empty coilset is an empty coilset ;)

if (CDsize == 0) return retval;

// If start is beyond the available coils, return empty slice

if (start > CDsize) return retval;

// length default is all up to the end

if (length == 0) length = CDsize - start;

// Does the requested slice fit in the buffer?

if ((start + length) <= CDsize) {

// Yes, it does. Extend return object

retval = CoilData(length);

// Loop over all requested bits

for (uint16_t i = start; i < start + length; ++i) {

if (CDbuffer[byteIndex(i)] & (1 << bitIndex(i))) {

retval.set(i - start, true);

}

return retval;

}

// operator[]: return value of a single coil

bool CoilData::operator[](uint16_t index) const {

if (index < CDsize) {

return (CDbuffer[byteIndex(index)] & (1 << bitIndex(index))) ? true : false;

}

// Wrong parameter -> always return false

return false;

}

// set functions to change coil value(s)

// Will return true if done, false if impossible (wrong address or data)

// set #1: alter one single coil

bool CoilData::set(uint16_t index, bool value) {

// Within coils?

if (index < CDsize) {

// Yes. Determine affected byte and bit therein

uint16_t by = byteIndex(index);

uint8_t mask = 1 << bitIndex(index);

// Stamp out bit

CDbuffer[by] &= ~mask;

// If required, set it to 1 now

if (value) {

CDbuffer[by] |= mask;

}

return true;

}

// Wrong parameter -> always return false

return false;

}

// set #2: alter a group of coils, overwriting it by the bits from vector newValue

bool CoilData::set(uint16_t start, uint16_t length, vector<uint8_t> newValue) {

// Does the vector contain enough data for the specified size?

if (newValue.size() >= (size_t)(byteIndex(length - 1) + 1)) {

// Yes, we safely may call set #3 with it

return set(start, length, newValue.data());

}

return false;

}

// set #3: alter a group of coils, overwriting it by the bits from uint8_t buffer newValue

// **** Watch out! ****

// This may be a potential risk if newValue is pointing to an array shorter than required.

// Then heap data behind the array may be used to set coils!

bool CoilData::set(uint16_t start, uint16_t length, uint8_t *newValue) {

// Does the requested slice fit in the buffer?

if (length && (start + length) <= CDsize) {

// Yes, it does.

// Prepare pointers to the source byte and the bit within

uint8_t *cp = newValue;

uint8_t bitPtr = 0;

// Loop over all bits to be set

for (uint16_t i = start; i < start + length; i++) {

// Get affected byte

uint8_t by = byteIndex(i);

// Calculate single-bit mask in target byte

uint8_t mask = 1 << bitIndex(i);

// Stamp out bit

CDbuffer[by] &= ~mask;

// is source bit set?

if (*cp & (1 << bitPtr)) {

// Yes. Set it in target as well

CDbuffer[by] |= mask;

}

// Advance source bit ptr

bitPtr++;

// Overflow?

if (bitPtr >= 8) {

// Yes. move pointers to first bit in next source byte

bitPtr = 0;

cp++;

}

return true;

}

return false;

}

// set #4: alter a group of coils, overwriting it by the coils in another CoilData object

// Setting stops when either target storage or source coils are exhausted

bool CoilData::set(uint16_t index, const CoilData& c) {

// if source object is empty, return false

if (c.empty()) return false;

// If target is empty, or index is beyond coils, return false

if (CDsize == 0 || index >= CDsize) return false;

// Take the minimum of remaining coils after index and the length of c

uint16_t length = CDsize - index;

if (c.coils() < length) length = c.coils();

// Loop over all coils to be copied

for (uint16_t i = index; i < index + length; ++i) {

set(i, c[i - index]);

}

return true;

}

// set #5: alter a group of coils, overwriting it by a bit image array

// Setting stops when either target storage or source bits are exhausted

bool CoilData::set(uint16_t index, const char *initVector) {

// if target is empty or index is beyond coils, return false

if (CDsize == 0 || index >= CDsize) return false;

// We do a single pass on the bit image array, until it ends or the target is exhausted

const char *cp = initVector; // pointer to source array

bool skipFlag = false; // Signal next character irrelevant

while (*cp && index < CDsize) {

switch (*cp) {

case '1': // A valid 1 bit

case '0': // A valid 0 bit

// Shall we ignore it?

if (skipFlag) {

// Yes. just reset the ignore flag

skipFlag = false;

} else {

// No, we can set it. First stamp out the existing bit

CDbuffer[byteIndex(index)] &= ~(1 << bitIndex(index));

// Do we have a 1 bit here?

if (*cp == '1') {

// Yes. set it in coil storage

CDbuffer[byteIndex(index)] |= (1 << bitIndex(index));

}

index++;

}

break;

case '_': // Skip next

skipFlag = true;

break;

default: // anything else

skipFlag = false;

break;

}

cp++;

}

return true;

}

// Comparison against bit image array

bool CoilData::operator==(const char *initVector) {

const char *cp = initVector; // pointer to source array

bool skipFlag = false; // Signal next character irrelevant

uint16_t index = 0;

// We do a single pass on the bit image array, until it ends or the target is exhausted

while (*cp && index < CDsize) {

switch (*cp) {

case '1': // A valid 1 bit

case '0': // A valid 0 bit

// Shall we ignore it?

if (skipFlag) {

// Yes. just reset the ignore flag

skipFlag = false;

} else {

// No, we can compare it

uint8_t value = CDbuffer[byteIndex(index)] & (1 << bitIndex(index));

// Do we have a 1 bit here?

if (*cp == '1') {

// Yes. Is the source different? Then we can stop

if (value == 0) return false;

} else {

// No, it is a 0. Different?

if (value) return false;

}

index++;

}

break;

case '_': // Skip next

skipFlag = true;

break;

default: // anything else

skipFlag = false;

break;

}

cp++;

}

// So far everything was equal, but we may have more bits in the image array!

if (*cp) {

// There is more. Check for more valid bits

while (*cp) {

switch (*cp) {

case '1': // A valid 1 bit

case '0': // A valid 0 bit

// Shall we ignore it?

if (skipFlag) {

// Yes. just reset the ignore flag

skipFlag = false;

} else {

// No, a valid bit that exceeds the target coils count

return false;

}

break;

case '_': // Skip next

skipFlag = true;

break;

default: // anything else

skipFlag = false;

break;

}

cp++;

}

return true;

}

bool CoilData::operator!=(const char *initVector) {

return !(*this == initVector);

}

// Init all coils by a readable bit image array

bool CoilData::setVector(const char *initVector) {

uint16_t length = 0; // resulting bit pattern length

const char *cp = initVector; // pointer to source array

bool skipFlag = false; // Signal next character irrelevant

// Do a first pass to count all valid bits in array

while (*cp) {

switch (*cp) {

case '1': // A valid 1 bit

case '0': // A valid 0 bit

// Shall we ignore it?

if (skipFlag) {

// Yes. just reset the ignore flag

skipFlag = false;

} else {

// No, we can count it

length++;

}

break;

case '_': // Skip next

skipFlag = true;

break;

default: // anything else

skipFlag = false;

break;

}

cp++;

}

// If there are coils already, trash them.

if (CDbuffer) {

delete CDbuffer;

}

CDsize = 0;

CDbyteSize = 0;

// Did we count a manageable number?

if (length && length <= 2000) {

// Yes. Init the coils

CDsize = length;

CDbyteSize = byteIndex(length - 1) + 1;

// Allocate new coil storage

CDbuffer = new uint8_t[CDbyteSize];

memset(CDbuffer, 0, CDbyteSize);

// Prepare second loop

uint16_t ptr = 0; // bit pointer in coil storage

skipFlag = false;

cp = initVector;

// Do a second pass, converting 1 and 0 into coils (bits)

// loop as above, only difference is setting the bits

while (*cp) {

switch (*cp) {

case '1':

case '0':

if (skipFlag) {

skipFlag = false;

} else {

// Do we have a 1 bit here?

if (*cp == '1') {

// Yes. set it in coil storage

CDbuffer[byteIndex(ptr)] |= (1 << bitIndex(ptr));

}

// advance bit pointer in any case 0 or 1

ptr++;

}

break;

case '_':

skipFlag = true;

break;

default:

skipFlag = false;

break;

}

cp++;

}

// We had content, so return true

return true;

}

// No valid bits found, return false

return false;

}

// init: set all coils to 1 or 0 (default)

void CoilData::init(bool value) {

if (CDsize > 0) {

memset(CDbuffer, value ? 0xFF : 0, CDbyteSize);

// Stamp out overhang bits

CDbuffer[CDbyteSize - 1] &= CDfilter[bitIndex(CDsize - 1)];

}

// Return number of coils set to 1 (or not)

// Uses Brian Kernighan's algorithm!

uint16_t CoilData::coilsSetON() const {

uint16_t count = 0;

// Do we have coils at all?

if (CDbyteSize) {

// Yes. Loop over all bytes summing up the '1' bits

for (uint8_t i = 0; i < CDbyteSize; ++i) {

uint8_t by = CDbuffer[i];

while (by) {

by &= by - 1; // this clears the LSB-most set bit

count++;

}

return count;

}

uint16_t CoilData::coilsSetOFF() const {

return CDsize - coilsSetON();

}

#if !IS_LINUX

// Not for Linux for the Print reference!

// Print out a coil storage in readable form to ease debugging

void CoilData::print(const char *label, Print& s) {

uint8_t bitptr = 0;

uint8_t labellen = strlen(label);

uint8_t pos = labellen;

// Put out the label

s.print(label);

// Print out all coils as "1" or "0"

for (uint16_t i = 0; i < CDsize; ++i) {

s.print((CDbuffer[byteIndex(i)] & (1 << bitptr)) ? "1" : "0");

pos++;

// Have a blank after every group of 4

if (i % 4 == 3) {

// Have a line break if > 80 characters, including the last group of 4

if (pos >= 80) {

s.println("");

pos = 0;

// Leave a nice empty space below the label

while (pos++ < labellen) {

s.print(" ");

}

} else {

s.print(" ");

pos++;

}

bitptr++;

bitptr &= 0x07;

}

s.println("");

}

#endif

eModbus/src/CoilData.cpp at master · eModbus/eModbus · GitHub

Uh oh!

Expand file tree

Breadcrumbs

CoilData.cpp

Latest commit

History

Breadcrumbs

CoilData.cpp

File metadata and controls