libetherpix

1) /*
2)  * EtherPix simulator
3)  *
4)  * Copyright 2017 Stefan Schuermans <stefan schuermans info>
5)  *
6)  * This program is free software: you can redistribute it and/or modify
7)  * it under the terms of the GNU General Public License as published by
8)  * the Free Software Foundation, version 3 of the License.
9)  *
10)  *
11)  * This program is distributed in the hope that it will be useful,
12)  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13)  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14)  * GNU General Public License for more details.
15)  *
16)  * You should have received a copy of the GNU Lesser General Public License
17)  * along with this program. If not, see <http://www.gnu.org/licenses/>.
18)  */
19) 

20) #include <arpa/inet.h>

21) #include <errno.h>

22) #include <gtkmm.h>

23) #include <map>

24) #include <sstream>
25) #include <stdexcept>

26) #include <stdint.h>
27) #include <string.h>

28) #include <vector>

29) 

30) #include "bbox.h"

31) #include "distri.h"

32) #include "mapping.h"
33) #include "pixel.h"

34) #include "transform.h"

35) 
36) /// constructor
37) Distri::Distri():
38)   m_distno(0),
39)   m_outputs(0),

40)   m_pixels(0),
41)   m_mappings(),

42)   m_outputMap(),
43)   m_sock(-1),
44)   m_packets(0)

45) {

46)   // initialize address to localhost and automatic port
47)   m_addr.sin_family = AF_INET;

48)   m_addr.sin_addr.s_addr = htonl(INADDR_LOOPBACK);

49)   m_addr.sin_port = htons(0);
50) }
51) 
52) /// destructor
53) Distri::~Distri()
54) {
55)   stop();

56) }
57) 
58) /**
59)  * @brief initialize
60)  * @param[in] distno distributor number
61)  * @param[in] outputs number of outputs
62)  * @param[in] pixels number of pixels per output
63)  */
64) void Distri::init(unsigned long distno, unsigned long outputs,
65)                   unsigned long pixels)
66) {
67)   m_distno = distno;
68)   m_outputs = outputs;
69)   m_pixels = pixels;
70) }
71) 

72) /**
73)  * @brief set network address
74)  * @param[in] addr IPv4/port network address (UDP)
75)  */
76) void Distri::setAddr(struct sockaddr_in const &addr)
77) {
78)   m_addr = addr;
79) }
80) 

81) /**
82)  * @brief get mapping
83)  * @param[in] chan color channel
84)  * @return mapping mapping object of color channel
85)  * @throws std::exception in case of error
86)  */
87) Mapping const& Distri::getMapping(Mapping::Channel chan) const
88) {
89)   if (chan >= Mapping::ChannelCount) {
90)     std::stringstream msg;
91)     msg << "invalid channel: " << chan;
92)     throw std::runtime_error(msg.str());
93)   }
94)   return m_mappings[chan];
95) }
96) 
97) /**
98)  * @brief set mapping
99)  * @param[in] chan color channel
100)  * @param[in] mapping new mapping object for color channel
101)  * @throws std::exception in case of error
102)  */
103) void Distri::setMapping(Mapping::Channel chan, Mapping const &mapping)
104) {
105)   if (chan >= Mapping::ChannelCount) {
106)     std::stringstream msg;
107)     msg << "invalid channel: " << chan;
108)     throw std::runtime_error(msg.str());
109)   }
110)   m_mappings[chan] = mapping;
111) }
112) 

113) /**
114)  * @brief add output to distributor
115)  * @param[in] outno number of output
116)  * @throws std::exception in case of error
117)  */
118) void Distri::addOutput(unsigned long outno)
119) {
120)   if (outno >= m_outputs) {
121)     std::stringstream msg;
122)     msg << "invalid output number " << outno << ": only " << m_outputs
123)         << " outputs supported";
124)     throw std::runtime_error(msg.str());
125)   }
126) 
127)   if (m_outputMap.find(outno) != m_outputMap.end()) {
128)     std::stringstream msg;
129)     msg << "output " << outno << " already exists";
130)     throw std::runtime_error(msg.str());
131)   }
132) 
133)   m_outputMap[outno];
134) }
135) 
136) /**
137)  * @brief add pixel to output of distributor
138)  * @param[in] outno number of output
139)  * @param[in] pixel pixel object to add
140)  * @throws std::exception in case of error
141)  */
142) void Distri::addPixel(unsigned long outno, Pixel const &pixel)
143) {
144)   if (m_outputMap.find(outno) == m_outputMap.end()) {
145)     std::stringstream msg;
146)     msg << "output " << outno << " does not exist";
147)     throw std::runtime_error(msg.str());
148)   }
149) 
150)   Output &output = m_outputMap[outno];
151) 
152)   if (output.m_pixels.size() >= m_pixels) {
153)     std::stringstream msg;
154)     msg << "cannot add pixel to output " << outno << ": already " << m_pixels
155)         << " pixels at this output (this is the maximum)";
156)     throw std::runtime_error(msg.str());
157)   }
158) 
159)   output.m_pixels.push_back(pixel);
160) }
161) 

162) /**
163)  * @brief add all pixels of this distributor to the bounding box
164)  * @param[in,out] bb bounding box
165)  */
166) void Distri::updateBBox(BBox &bb) const
167) {
168)   OutputMap::const_iterator out;
169)   for (out = m_outputMap.begin(); out != m_outputMap.end(); ++out) {
170)     Output const &output = out->second;
171)     std::vector<Pixel>::const_iterator pix;
172)     for (pix = output.m_pixels.begin(); pix != output.m_pixels.end(); ++pix) {
173)       Pixel const &pixel = *pix;
174)       pixel.updateBBox(bb);
175)     }
176)   }
177) }
178) 

179) /**
180)  * @brief create socket and listen for incoming packets
181)  * @throws std::exception in case of error
182)  */
183) void Distri::start()
184) {
185)   // leave if  socket already exists
186)   if (m_sock != -1) {
187)     return;
188)   }
189) 
190)   // create socket
191)   m_sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
192)   if (m_sock == -1) {
193)     std::stringstream msg;
194)     msg << "could not create IPv4/UDP socket: " << strerror(errno);
195)     throw::std::runtime_error(msg.str());
196)   }
197)   try {
198) 
199)     // bind socket
200)     if (bind(m_sock, (struct sockaddr *)&m_addr, sizeof(m_addr)) != 0) {
201)       std::stringstream msg;
202)       msg << "could not bind IPv4/UDP socket to \""
203)           << inet_ntoa(m_addr.sin_addr) << ":" << ntohs(m_addr.sin_port)
204)           << "\": " << strerror(errno);
205)       throw::std::runtime_error(msg.str());
206)     }
207) 
208)     // register callback on incomimg packets
209)     m_conn = Glib::signal_io().connect(sigc::mem_fun(*this, &Distri::on_packet),
210)                                        m_sock, Glib::IO_IN);
211) 
212)   } catch (...) {
213)     close(m_sock);
214)     m_sock = -1;
215)     throw;
216)   }
217) }
218) 
219) /**
220)  * @brief stop listening for incoming packets and close socket
221)  */
222) void Distri::stop()
223) {
224)   // leave if no socket
225)   if (m_sock == -1) {
226)     return;
227)   }
228) 
229)   // uninstall callback
230)   m_conn.disconnect();
231) 
232)   // close socket
233)   close(m_sock);
234) 
235)   // socket in no more there
236)   m_sock = -1;
237) }
238) 
239) /// get and reset packet conter
240) unsigned long Distri::getPacketCounterAndReset()
241) {
242)   unsigned long packets = m_packets;
243)   m_packets = 0;
244)   return packets;
245) }
246) 

247) /**
248)  * @brief draw pixels of this distributor
249)  * @param[in] cairo cairo context for drawing
250)  * @param[in] tf coordinate transformation
251)  */
252) void Distri::draw(Cairo::RefPtr<Cairo::Context> &cairo,
253)                   Transform const &tf) const
254) {
255)   OutputMap::const_iterator out;
256)   for (out = m_outputMap.begin(); out != m_outputMap.end(); ++out) {
257)     Output const &output = out->second;
258)     std::vector<Pixel>::const_iterator pix;
259)     for (pix = output.m_pixels.begin(); pix != output.m_pixels.end(); ++pix) {
260)       Pixel const &pixel = *pix;
261)       pixel.draw(cairo, tf);
262)     }
263)   }
264) }
265) 

266) /**
267)  * @brief callback on packet input on socket
268)  */
269) bool Distri::on_packet(Glib::IOCondition condition)
270) {
271)   // incoming packet
272)   if (condition & Glib::IO_IN) {
273) 
274)     // read packet
275)     uint8_t buffer[65536];
276)     ssize_t sz = recv(m_sock, buffer, sizeof(buffer), 0);
277)     // success and long enough for header
278)     if (sz >= 12) {

279)       // check magic, 3 channels and maxval 0xFF

280)       if (buffer[ 0] == 0x23 && buffer[ 1] == 0x54 &&
281)           buffer[ 2] == 0x26 && buffer[ 3] == 0x66 &&

282)           buffer[ 8] == 0x00 && (buffer[9] == 0x01 || buffer[9] == 0x03) &&

283)           buffer[10] == 0x00 && buffer[11] == 0xFF) {

284)         // get outputs (height), pixels (width) and channels

285)         unsigned int outputs = (unsigned int)buffer[4] << 8 |
286)                                (unsigned int)buffer[5];
287)         unsigned int pixels = (unsigned int)buffer[6] << 8 |
288)                               (unsigned int)buffer[7];

289)         unsigned int channels = buffer[9];

290)         // check data length

291)         if ((size_t)sz >= 12 + outputs * pixels * channels) {

292)           uint8_t const *data = buffer + 12;
293)           // process pixel data

294)           procPixelData(outputs, pixels, channels, data);

295)         }
296)       }
297)     }
298) 
299)   } // if IO_IN
300) 
301)   // keep callback installed
302)   return true;
303) }
304) 
305) /**
306)  * @brief process pixel data
307)  * @param[in] outputs number of outputs in pixel data
308)  * @param[in] pixels number of pixels per output in pixel data

309)  * @param[in] channels number of channels in pixel data

310)  * @param[in] data pixel data
311)  */
312) void Distri::procPixelData(unsigned long outputs, unsigned long pixels,

313)                            unsigned int channels, uint8_t const *data)

314) {
315)   // get mappings
316)   Mapping const & mapRed   = m_mappings[Mapping::Red  ];
317)   Mapping const & mapGreen = m_mappings[Mapping::Green];
318)   Mapping const & mapBlue  = m_mappings[Mapping::Blue ];
319) 
320)   // traverse all outputs and pixels

321)   unsigned int ofs_r = 0;
322)   unsigned int ofs_g = channels >= 2 ? 1 : 0;
323)   unsigned int ofs_b = channels >= 3 ? 2 : 0;

324)   OutputMap::iterator out = m_outputMap.begin();
325)   OutputMap::iterator outend = m_outputMap.end();
326)   unsigned long o = 0;
327)   unsigned long oi = 0;

328)   for (; out != outend; ++out, ++o, oi += pixels * channels) {

329)     Output &output = out->second;
330)     std::vector<Pixel>::iterator pix = output.m_pixels.begin();
331)     std::vector<Pixel>::iterator pixend = output.m_pixels.end();
332)     unsigned long p = 0;
333)     unsigned long pi = oi;

334)     for (; pix != pixend; ++pix, ++p, pi += channels) {

335)       Pixel &pixel = *pix;
336) 
337)       // default color: black (in case pixel is not in pixel data)
338)       uint8_t r = 0, g = 0, b = 0;
339)       // get color of pixel from data
340)       if (o < outputs && p < pixels) {

341)         r = mapRed  .display2video(data[pi + ofs_r]);
342)         g = mapGreen.display2video(data[pi + ofs_g]);
343)         b = mapBlue .display2video(data[pi + ofs_b]);